Refactor long (#21433)

* refactor

* needs casting

* tests pass

* fix that test

* refactor in controls

* lets not go crazy

* change of names

* use constants

* better naming

* renamed

* soft constraints

* compile slack variables

* rm git conflict

* add slack variables

* unused

* new edition

* fcw

* fix tests

* dividing causes problems

* was way too slow

* take a step back

* byeeee

* for another time

* bad idxs

* little more cpu for cruise mpc

* update refs

* these limits seem fine

* rename

* test model timings fails sometimes

* add default

* save some cpu

* Revert "little more cpu for cruise mpc"

This reverts commit f0a8163ec90e8dc1eabb3c4a4268ad330d23374d.

* Revert "test model timings fails sometimes"

This reverts commit d259d845710ed2cbeb28b383e2600476527d4838.

* update refs

* less cpu

* Revert "Revert "test model timings fails sometimes""

This reverts commit e0263050d9929bfc7ee70c9788234541a4a8461c.

* Revert "less cpu"

This reverts commit 679007472bc2013e7fafb7b17de7a43d6f82359a.

* cleanup

* not too much until we clean up mpc

* more cost on jerk

* change ref

* add todo

* new ref

* indentation
old-commit-hash: be5ddd25cd

SConstruct

@@ -402,8 +402,8 @@ SConscript(['selfdrive/modeld/SConscript'])
 
 SConscript(['selfdrive/controls/lib/cluster/SConscript'])
 SConscript(['selfdrive/controls/lib/lateral_mpc/SConscript'])
-SConscript(['selfdrive/controls/lib/longitudinal_mpc/SConscript'])
-SConscript(['selfdrive/controls/lib/longitudinal_mpc_model/SConscript'])
+SConscript(['selfdrive/controls/lib/lead_mpc_lib/SConscript'])
+SConscript(['selfdrive/controls/lib/longitudinal_mpc_lib/SConscript'])
 
 SConscript(['selfdrive/boardd/SConscript'])
 SConscript(['selfdrive/proclogd/SConscript'])

cereal

@@ -1 +1 @@
-Subproject commit 1f5c4aa350c3783f249724e7a4ea0049e4c46a7a
+Subproject commit 1979127659dc7c47c6ad7b8234ff1f6ca93526fc

release/files_common

@@ -238,10 +238,9 @@ selfdrive/controls/lib/pid.py
 selfdrive/controls/lib/longitudinal_planner.py
 selfdrive/controls/lib/radar_helpers.py
 selfdrive/controls/lib/vehicle_model.py
-selfdrive/controls/lib/speed_smoother.py
 selfdrive/controls/lib/fcw.py
 selfdrive/controls/lib/long_mpc.py
-selfdrive/controls/lib/long_mpc_model.py
+selfdrive/controls/lib/lead_mpc.py
 
 selfdrive/controls/lib/cluster/*
 
@@ -253,21 +252,21 @@ selfdrive/controls/lib/lateral_mpc/generator.cpp
 selfdrive/controls/lib/lateral_mpc/libmpc_py.py
 selfdrive/controls/lib/lateral_mpc/lateral_mpc.c
 
-selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/*
-selfdrive/controls/lib/longitudinal_mpc/.gitignore
-selfdrive/controls/lib/longitudinal_mpc/SConscript
-selfdrive/controls/lib/longitudinal_mpc/__init__.py
-selfdrive/controls/lib/longitudinal_mpc/generator.cpp
-selfdrive/controls/lib/longitudinal_mpc/libmpc_py.py
-selfdrive/controls/lib/longitudinal_mpc/longitudinal_mpc.c
-
-selfdrive/controls/lib/longitudinal_mpc_model/lib_mpc_export/*
-selfdrive/controls/lib/longitudinal_mpc_model/.gitignore
-selfdrive/controls/lib/longitudinal_mpc_model/SConscript
-selfdrive/controls/lib/longitudinal_mpc_model/__init__.py
-selfdrive/controls/lib/longitudinal_mpc_model/generator.cpp
-selfdrive/controls/lib/longitudinal_mpc_model/libmpc_py.py
-selfdrive/controls/lib/longitudinal_mpc_model/longitudinal_mpc.c
+selfdrive/controls/lib/lead_mpc_lib/lib_mpc_export/*
+selfdrive/controls/lib/lead_mpc_lib/.gitignore
+selfdrive/controls/lib/lead_mpc_lib/SConscript
+selfdrive/controls/lib/lead_mpc_lib/__init__.py
+selfdrive/controls/lib/lead_mpc_lib/generator.cpp
+selfdrive/controls/lib/lead_mpc_lib/libmpc_py.py
+selfdrive/controls/lib/lead_mpc_lib/longitudinal_mpc.c
+
+selfdrive/controls/lib/longitudinal_mpc_lib/lib_mpc_export/*
+selfdrive/controls/lib/longitudinal_mpc_lib/.gitignore
+selfdrive/controls/lib/longitudinal_mpc_lib/SConscript
+selfdrive/controls/lib/longitudinal_mpc_lib/__init__.py
+selfdrive/controls/lib/longitudinal_mpc_lib/generator.cpp
+selfdrive/controls/lib/longitudinal_mpc_lib/libmpc_py.py
+selfdrive/controls/lib/longitudinal_mpc_lib/longitudinal_mpc.c
 
 selfdrive/hardware/__init__.py
 selfdrive/hardware/base.h

selfdrive/car/honda/interface.py

@@ -8,10 +8,9 @@ from selfdrive.config import Conversions as CV
 from selfdrive.car.honda.values import CruiseButtons, CAR, HONDA_BOSCH, HONDA_BOSCH_ALT_BRAKE_SIGNAL
 from selfdrive.car.honda.hondacan import disable_radar
 from selfdrive.car import STD_CARGO_KG, CivicParams, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint
-from selfdrive.controls.lib.longitudinal_planner import _A_CRUISE_MAX_V_FOLLOWING
+from selfdrive.controls.lib.longitudinal_planner import A_CRUISE_MAX as A_ACC_MAX
 from selfdrive.car.interfaces import CarInterfaceBase
 
-A_ACC_MAX = max(_A_CRUISE_MAX_V_FOLLOWING)
 
 ButtonType = car.CarState.ButtonEvent.Type
 EventName = car.CarEvent.EventName

selfdrive/common/modeldata.h

@@ -1,5 +1,7 @@
 #pragma once
 const int  TRAJECTORY_SIZE = 33;
+const int LON_MPC_N = 32;
+const int LAT_MPC_N = 16;
 const float MIN_DRAW_DISTANCE = 10.0;
 const float MAX_DRAW_DISTANCE = 100.0;
 

selfdrive/controls/controlsd.py

@@ -22,7 +22,6 @@ from selfdrive.controls.lib.latcontrol_angle import LatControlAngle
 from selfdrive.controls.lib.events import Events, ET
 from selfdrive.controls.lib.alertmanager import AlertManager
 from selfdrive.controls.lib.vehicle_model import VehicleModel
-from selfdrive.controls.lib.longitudinal_planner import LON_MPC_STEP
 from selfdrive.locationd.calibrationd import Calibration
 from selfdrive.hardware import HARDWARE, TICI
 
@@ -143,6 +142,8 @@ class Controls:
     self.events_prev = []
     self.current_alert_types = [ET.PERMANENT]
     self.logged_comm_issue = False
+    self.v_target = 0.0
+    self.a_target = 0.0
 
     # TODO: no longer necessary, aside from process replay
     self.sm['liveParameters'].valid = True
@@ -304,7 +305,12 @@ class Controls:
         self.events.add(EventName.processNotRunning)
 
     # Only allow engagement with brake pressed when stopped behind another stopped car
-    if CS.brakePressed and self.sm['longitudinalPlan'].vTargetFuture >= STARTING_TARGET_SPEED \
+    speeds = self.sm['longitudinalPlan'].speeds
+    if len(speeds) > 1:
+      v_future = speeds[-1]
+    else:
+      v_future = 100.0
+    if CS.brakePressed and v_future >= STARTING_TARGET_SPEED \
       and self.CP.openpilotLongitudinalControl and CS.vEgo < 0.3:
       self.events.add(EventName.noTarget)
 
@@ -444,16 +450,9 @@ class Controls:
       self.LaC.reset()
       self.LoC.reset(v_pid=CS.vEgo)
 
-    long_plan_age = DT_CTRL * (self.sm.frame - self.sm.rcv_frame['longitudinalPlan'])
-    # no greater than dt mpc + dt, to prevent too high extraps
-    dt = min(long_plan_age, LON_MPC_STEP + DT_CTRL) + DT_CTRL
-
-    a_acc_sol = long_plan.aStart + (dt / LON_MPC_STEP) * (long_plan.aTarget - long_plan.aStart)
-    v_acc_sol = long_plan.vStart + dt * (a_acc_sol + long_plan.aStart) / 2.0
-
     if not self.joystick_mode:
       # Gas/Brake PID loop
-      actuators.gas, actuators.brake = self.LoC.update(self.active, CS, v_acc_sol, long_plan.vTargetFuture, a_acc_sol, self.CP)
+      actuators.gas, actuators.brake, self.v_target, self.a_target = self.LoC.update(self.active, CS, self.CP, long_plan)
 
       # Steering PID loop and lateral MPC
       desired_curvature, desired_curvature_rate = get_lag_adjusted_curvature(self.CP, CS.vEgo,
@@ -498,9 +497,9 @@ class Controls:
         if left_deviation or right_deviation:
           self.events.add(EventName.steerSaturated)
 
-    return actuators, v_acc_sol, a_acc_sol, lac_log
+    return actuators, lac_log
 
-  def publish_logs(self, CS, start_time, actuators, v_acc, a_acc, lac_log):
+  def publish_logs(self, CS, start_time, actuators, lac_log):
     """Send actuators and hud commands to the car, send controlsstate and MPC logging"""
 
     CC = car.CarControl.new_message()
@@ -509,16 +508,17 @@ class Controls:
 
     CC.cruiseControl.override = True
     CC.cruiseControl.cancel = not self.CP.enableCruise or (not self.enabled and CS.cruiseState.enabled)
-
     if self.joystick_mode and self.sm.rcv_frame['testJoystick'] > 0 and self.sm['testJoystick'].buttons[0]:
       CC.cruiseControl.cancel = True
 
+    # TODO remove car specific stuff in controls
     # Some override values for Honda
     # brake discount removes a sharp nonlinearity
     brake_discount = (1.0 - clip(actuators.brake * 3., 0.0, 1.0))
     speed_override = max(0.0, (self.LoC.v_pid + CS.cruiseState.speedOffset) * brake_discount)
     CC.cruiseControl.speedOverride = float(speed_override if self.CP.enableCruise else 0.0)
-    CC.cruiseControl.accelOverride = self.CI.calc_accel_override(CS.aEgo, self.sm['longitudinalPlan'].aTarget, CS.vEgo, self.sm['longitudinalPlan'].vTarget)
+    CC.cruiseControl.accelOverride = float(self.CI.calc_accel_override(CS.aEgo, self.a_target,
+                                                                       CS.vEgo, self.v_target))
 
     CC.hudControl.setSpeed = float(self.v_cruise_kph * CV.KPH_TO_MS)
     CC.hudControl.speedVisible = self.enabled
@@ -591,8 +591,6 @@ class Controls:
     controlsState.upAccelCmd = float(self.LoC.pid.p)
     controlsState.uiAccelCmd = float(self.LoC.pid.i)
     controlsState.ufAccelCmd = float(self.LoC.pid.f)
-    controlsState.vTargetLead = float(v_acc)
-    controlsState.aTarget = float(a_acc)
     controlsState.cumLagMs = -self.rk.remaining * 1000.
     controlsState.startMonoTime = int(start_time * 1e9)
     controlsState.forceDecel = bool(force_decel)
@@ -656,12 +654,12 @@ class Controls:
       self.prof.checkpoint("State transition")
 
     # Compute actuators (runs PID loops and lateral MPC)
-    actuators, v_acc, a_acc, lac_log = self.state_control(CS)
+    actuators, lac_log = self.state_control(CS)
 
     self.prof.checkpoint("State Control")
 
     # Publish data
-    self.publish_logs(CS, start_time, actuators, v_acc, a_acc, lac_log)
+    self.publish_logs(CS, start_time, actuators, lac_log)
     self.prof.checkpoint("Sent")
 
   def controlsd_thread(self):

selfdrive/controls/lib/drive_helpers.py

@@ -11,6 +11,7 @@ V_CRUISE_MIN = 8
 V_CRUISE_DELTA = 8
 V_CRUISE_ENABLE_MIN = 40
 LAT_MPC_N = 16
+LON_MPC_N = 32
 CONTROL_N = 17
 CAR_ROTATION_RADIUS = 0.0
 

selfdrive/controls/lib/lateral_mpc/generator.cpp

@@ -4,7 +4,6 @@
 
 #define deg2rad(d) (d/180.0*M_PI)
 
-const int N_steps = 16;
 using namespace std;
 
 int main( )
@@ -26,8 +25,11 @@ int main( )
 
 
   // Equations of motion
-  f << dot(xx) == v_ego * cos(psi) - rotation_radius * sin(psi) * (v_ego * curvature);
-  f << dot(yy) == v_ego * sin(psi) + rotation_radius * cos(psi) * (v_ego * curvature);
+  f << dot(xx) == v_ego * cos(psi);
+  f << dot(yy) == v_ego * sin(psi);
+  // disable rotation radius for now
+  //f << dot(xx) == v_ego * cos(psi) - rotation_radius * sin(psi) * (v_ego * curvature);
+  //f << dot(yy) == v_ego * sin(psi) + rotation_radius * cos(psi) * (v_ego * curvature);
   f << dot(psi) == v_ego * curvature;
   f << dot(curvature) == curvature_rate;
 
@@ -65,9 +67,9 @@ int main( )
   // QN(2,2) = 1.0;
   // QN(3,3) = 1.0;
 
-  double T_IDXS_ARR[N_steps + 1];
-  memcpy(T_IDXS_ARR, T_IDXS, (N_steps + 1) * sizeof(double));
-  Grid times(N_steps + 1, T_IDXS_ARR);
+  double T_IDXS_ARR[LAT_MPC_N + 1];
+  memcpy(T_IDXS_ARR, T_IDXS, (LAT_MPC_N + 1) * sizeof(double));
+  Grid times(LAT_MPC_N + 1, T_IDXS_ARR);
   OCP ocp(times);
   ocp.subjectTo(f);
 
@@ -84,8 +86,8 @@ int main( )
   mpc.set( HESSIAN_APPROXIMATION, GAUSS_NEWTON );
   mpc.set( DISCRETIZATION_TYPE, MULTIPLE_SHOOTING );
   mpc.set( INTEGRATOR_TYPE, INT_RK4 );
-  mpc.set( NUM_INTEGRATOR_STEPS, 2500);
-  mpc.set( MAX_NUM_QP_ITERATIONS, 1000);
+  mpc.set( NUM_INTEGRATOR_STEPS, 1000);
+  mpc.set( MAX_NUM_QP_ITERATIONS, 50);
   mpc.set( CG_USE_VARIABLE_WEIGHTING_MATRIX, YES);
 
   mpc.set( SPARSE_QP_SOLUTION, CONDENSING );

selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_common.h

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:1879f4c6bd1474b9de2ecf966781598e1bdbefea7f5e7c064da647df9d401133
+oid sha256:0632df8816a04953959a26e6a5ac5a849dcb3db904ea5f2c31f8b92507662291
 size 8752

selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_integrator.c

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:4f02dc89c46b658a91722f8047abbe258bf051d46e63dbbb26f662a235149f34
-size 19058
+oid sha256:cc6d5413d58f774a3a288573935f6cec938b053475f094b8642de5c6a31c682e
+size 17982

selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_qpoases_interface.hpp

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:030e60a7796b3730a96d7157800ecc2d2390b8dbe2ebcd81a849b490cce3942a
-size 1822
+oid sha256:d7cc184d2cda2505daa0a122c5396df707ece4f9d1870f59d4e3b6b1bdc8fd63
+size 1820

selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_solver.c

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:4d7de5aed6e5014ec35745f92356850f5b5d31b226cbcf93e9fafe0a4e29118c
+oid sha256:3d24c95e8374827a8f5959019f3758d4e2c49f29559f5ec23aa359078108b8d2
 size 243595

selfdrive/controls/lib/lateral_planner.py

@@ -176,6 +176,9 @@ class LateralPlanner():
 
     assert len(y_pts) == LAT_MPC_N + 1
     assert len(heading_pts) == LAT_MPC_N + 1
+    # for now CAR_ROTATION_RADIUS is disabled
+    # to use it, enable it in the MPC
+    assert abs(CAR_ROTATION_RADIUS) < 1e-3
     self.libmpc.run_mpc(self.cur_state, self.mpc_solution,
                         float(v_ego),
                         CAR_ROTATION_RADIUS,

selfdrive/controls/lib/lead_mpc.py

@@ -0,0 +1,106 @@
+import math
+import numpy as np
+from common.numpy_fast import interp
+from common.realtime import sec_since_boot
+from selfdrive.modeld.constants import T_IDXS
+from selfdrive.controls.lib.radar_helpers import _LEAD_ACCEL_TAU
+from selfdrive.controls.lib.lead_mpc_lib import libmpc_py
+from selfdrive.controls.lib.drive_helpers import MPC_COST_LONG, CONTROL_N
+from selfdrive.swaglog import cloudlog
+
+MPC_T = list(np.arange(0,1.,.2)) + list(np.arange(1.,10.6,.6))
+
+
+class LeadMpc():
+  def __init__(self, mpc_id):
+    self.lead_id = mpc_id
+
+    self.reset_mpc()
+    self.prev_lead_status = False
+    self.prev_lead_x = 0.0
+    self.new_lead = False
+
+    self.last_cloudlog_t = 0.0
+    self.n_its = 0
+    self.duration = 0
+    self.status = False
+
+  def reset_mpc(self):
+    ffi, self.libmpc = libmpc_py.get_libmpc(self.lead_id)
+    self.libmpc.init(MPC_COST_LONG.TTC, MPC_COST_LONG.DISTANCE,
+                     MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK)
+
+    self.mpc_solution = ffi.new("log_t *")
+    self.cur_state = ffi.new("state_t *")
+    self.cur_state[0].v_ego = 0
+    self.cur_state[0].a_ego = 0
+    self.a_lead_tau = _LEAD_ACCEL_TAU
+
+  def set_cur_state(self, v, a):
+    v_safe = max(v, 1e-3)
+    a_safe = a
+    self.cur_state[0].v_ego = v_safe
+    self.cur_state[0].a_ego = a_safe
+
+  def update(self, CS, radarstate, v_cruise):
+    v_ego = CS.vEgo
+    if self.lead_id == 0:
+      lead = radarstate.leadOne
+    else:
+      lead = radarstate.leadOne
+    self.status = lead.status and lead.modelProb > .5
+
+    # Setup current mpc state
+    self.cur_state[0].x_ego = 0.0
+
+    if lead is not None and lead.status:
+      x_lead = lead.dRel
+      v_lead = max(0.0, lead.vLead)
+      a_lead = lead.aLeadK
+
+      if (v_lead < 0.1 or -a_lead / 2.0 > v_lead):
+        v_lead = 0.0
+        a_lead = 0.0
+
+      self.a_lead_tau = lead.aLeadTau
+      self.new_lead = False
+      if not self.prev_lead_status or abs(x_lead - self.prev_lead_x) > 2.5:
+        self.libmpc.init_with_simulation(v_ego, x_lead, v_lead, a_lead, self.a_lead_tau)
+        self.new_lead = True
+
+      self.prev_lead_status = True
+      self.prev_lead_x = x_lead
+      self.cur_state[0].x_l = x_lead
+      self.cur_state[0].v_l = v_lead
+    else:
+      self.prev_lead_status = False
+      # Fake a fast lead car, so mpc keeps running
+      self.cur_state[0].x_l = 50.0
+      self.cur_state[0].v_l = v_ego + 10.0
+      a_lead = 0.0
+      self.a_lead_tau = _LEAD_ACCEL_TAU
+
+    # Calculate mpc
+    t = sec_since_boot()
+    self.n_its = self.libmpc.run_mpc(self.cur_state, self.mpc_solution, self.a_lead_tau, a_lead)
+    self.v_solution = interp(T_IDXS[:CONTROL_N], MPC_T, self.mpc_solution.v_ego)
+    self.a_solution = interp(T_IDXS[:CONTROL_N], MPC_T, self.mpc_solution.a_ego)
+    self.duration = int((sec_since_boot() - t) * 1e9)
+
+    # Reset if NaN or goes through lead car
+    crashing = any(lead - ego < -50 for (lead, ego) in zip(self.mpc_solution[0].x_l, self.mpc_solution[0].x_ego))
+    nans = any(math.isnan(x) for x in self.mpc_solution[0].v_ego)
+    backwards = min(self.mpc_solution[0].v_ego) < -0.01
+
+    if ((backwards or crashing) and self.prev_lead_status) or nans:
+      if t > self.last_cloudlog_t + 5.0:
+        self.last_cloudlog_t = t
+        cloudlog.warning("Longitudinal mpc %d reset - backwards: %s crashing: %s nan: %s" % (
+                          self.lead_id, backwards, crashing, nans))
+
+      self.libmpc.init(MPC_COST_LONG.TTC, MPC_COST_LONG.DISTANCE,
+                       MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK)
+      self.cur_state[0].v_ego = v_ego
+      self.cur_state[0].a_ego = 0.0
+      self.a_mpc = CS.aEgo
+      self.prev_lead_status = False

selfdrive/controls/lib/lead_mpc_lib/SConscript

@@ -44,5 +44,5 @@ if GetOption('mpc_generate'):
 
 
 mpc_files = ["longitudinal_mpc.c"] + generated_c
+env.SharedLibrary('mpc0', mpc_files, LIBS=['m', 'qpoases'], LIBPATH=['lib_qp'], CPPPATH=cpp_path)
 env.SharedLibrary('mpc1', mpc_files, LIBS=['m', 'qpoases'], LIBPATH=['lib_qp'], CPPPATH=cpp_path)
-env.SharedLibrary('mpc2', mpc_files, LIBS=['m', 'qpoases'], LIBPATH=['lib_qp'], CPPPATH=cpp_path)

selfdrive/controls/lib/lead_mpc_lib/generator.cpp

@@ -77,7 +77,7 @@ int main( )
   mpc.set( DISCRETIZATION_TYPE, MULTIPLE_SHOOTING );
   mpc.set( INTEGRATOR_TYPE, INT_RK4 );
   mpc.set( NUM_INTEGRATOR_STEPS, controlHorizon);
-  mpc.set( MAX_NUM_QP_ITERATIONS, 500);
+  mpc.set( MAX_NUM_QP_ITERATIONS, 50);
   mpc.set( CG_USE_VARIABLE_WEIGHTING_MATRIX, YES);
 
   mpc.set( SPARSE_QP_SOLUTION, CONDENSING );

selfdrive/controls/lib/lead_mpc_lib/lib_mpc_export/acado_qpoases_interface.hpp

@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:333aa4c0fbe9e0c5b7e75df6b4366a102bb53861b7ae3f0eeb53a3ddf575788f
+size 1820

selfdrive/controls/lib/lead_mpc_lib/libmpc_py.py

@@ -35,7 +35,7 @@ def _get_libmpc(mpc_id):
 
     return (ffi, ffi.dlopen(libmpc_fn))
 
-mpcs = [_get_libmpc(1), _get_libmpc(2)]
+mpcs = [_get_libmpc(0), _get_libmpc(1)]
 
 def get_libmpc(mpc_id):
-    return mpcs[mpc_id - 1]
+    return mpcs[mpc_id]

selfdrive/controls/lib/long_mpc.py

@@ -1,122 +1,66 @@
-import os
+import numpy as np
 import math
 
-import cereal.messaging as messaging
 from selfdrive.swaglog import cloudlog
 from common.realtime import sec_since_boot
-from selfdrive.controls.lib.radar_helpers import _LEAD_ACCEL_TAU
-from selfdrive.controls.lib.longitudinal_mpc import libmpc_py
-from selfdrive.controls.lib.drive_helpers import MPC_COST_LONG
-
-LOG_MPC = os.environ.get('LOG_MPC', False)
+from selfdrive.controls.lib.longitudinal_mpc_lib import libmpc_py
+from selfdrive.controls.lib.drive_helpers import LON_MPC_N
+from selfdrive.modeld.constants import T_IDXS
 
 
 class LongitudinalMpc():
-  def __init__(self, mpc_id):
-    self.mpc_id = mpc_id
-
-    self.setup_mpc()
-    self.v_mpc = 0.0
-    self.v_mpc_future = 0.0
-    self.a_mpc = 0.0
-    self.v_cruise = 0.0
-    self.prev_lead_status = False
-    self.prev_lead_x = 0.0
-    self.new_lead = False
-
+  def __init__(self):
+    self.reset_mpc()
     self.last_cloudlog_t = 0.0
-    self.n_its = 0
-    self.duration = 0
+    self.ts = list(range(10))
+    self.status = True
+    self.min_a = -1.2
+    self.max_a = 1.2
+
 
-  def publish(self, pm):
-    if LOG_MPC:
-      qp_iterations = max(0, self.n_its)
-      dat = messaging.new_message('liveLongitudinalMpc')
-      dat.liveLongitudinalMpc.xEgo = list(self.mpc_solution[0].x_ego)
-      dat.liveLongitudinalMpc.vEgo = list(self.mpc_solution[0].v_ego)
-      dat.liveLongitudinalMpc.aEgo = list(self.mpc_solution[0].a_ego)
-      dat.liveLongitudinalMpc.xLead = list(self.mpc_solution[0].x_l)
-      dat.liveLongitudinalMpc.vLead = list(self.mpc_solution[0].v_l)
-      dat.liveLongitudinalMpc.cost = self.mpc_solution[0].cost
-      dat.liveLongitudinalMpc.aLeadTau = self.a_lead_tau
-      dat.liveLongitudinalMpc.qpIterations = qp_iterations
-      dat.liveLongitudinalMpc.mpcId = self.mpc_id
-      dat.liveLongitudinalMpc.calculationTime = self.duration
-      pm.send('liveLongitudinalMpc', dat)
+  def reset_mpc(self):
+    self.libmpc = libmpc_py.libmpc
+    self.libmpc.init(0.0, 1.0, 0.0, 50.0, 10000.0)
 
-  def setup_mpc(self):
-    ffi, self.libmpc = libmpc_py.get_libmpc(self.mpc_id)
-    self.libmpc.init(MPC_COST_LONG.TTC, MPC_COST_LONG.DISTANCE,
-                     MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK)
+    self.mpc_solution = libmpc_py.ffi.new("log_t *")
+    self.cur_state = libmpc_py.ffi.new("state_t *")
 
-    self.mpc_solution = ffi.new("log_t *")
-    self.cur_state = ffi.new("state_t *")
+    self.cur_state[0].x_ego = 0
     self.cur_state[0].v_ego = 0
     self.cur_state[0].a_ego = 0
-    self.a_lead_tau = _LEAD_ACCEL_TAU
 
-  def set_cur_state(self, v, a):
-    self.cur_state[0].v_ego = v
-    self.cur_state[0].a_ego = a
-
-  def update(self, CS, lead):
-    v_ego = CS.vEgo
+  def set_accel_limits(self, min_a, max_a):
+    self.min_a = min_a
+    self.max_a = max_a
 
-    # Setup current mpc state
+  def set_cur_state(self, v, a):
+    v_safe = max(v, 1e-2)
+    a_safe = min(a, self.max_a - 1e-2)
+    a_safe = max(a_safe, self.min_a + 1e-2)
     self.cur_state[0].x_ego = 0.0
+    self.cur_state[0].v_ego = v_safe
+    self.cur_state[0].a_ego = a_safe
 
-    if lead is not None and lead.status:
-      x_lead = lead.dRel
-      v_lead = max(0.0, lead.vLead)
-      a_lead = lead.aLeadK
-
-      if (v_lead < 0.1 or -a_lead / 2.0 > v_lead):
-        v_lead = 0.0
-        a_lead = 0.0
-
-      self.a_lead_tau = lead.aLeadTau
-      self.new_lead = False
-      if not self.prev_lead_status or abs(x_lead - self.prev_lead_x) > 2.5:
-        self.libmpc.init_with_simulation(self.v_mpc, x_lead, v_lead, a_lead, self.a_lead_tau)
-        self.new_lead = True
-
-      self.prev_lead_status = True
-      self.prev_lead_x = x_lead
-      self.cur_state[0].x_l = x_lead
-      self.cur_state[0].v_l = v_lead
-    else:
-      self.prev_lead_status = False
-      # Fake a fast lead car, so mpc keeps running
-      self.cur_state[0].x_l = 50.0
-      self.cur_state[0].v_l = v_ego + 10.0
-      a_lead = 0.0
-      self.a_lead_tau = _LEAD_ACCEL_TAU
+  def update(self, carstate, model, v_cruise):
+    v_cruise_clipped = np.clip(v_cruise, self.cur_state[0].v_ego - 10., self.cur_state[0].v_ego + 10.0)
+    poss = v_cruise_clipped * np.array(T_IDXS[:LON_MPC_N+1])
+    speeds = v_cruise_clipped * np.ones(LON_MPC_N+1)
+    accels = np.zeros(LON_MPC_N+1)
 
     # Calculate mpc
-    t = sec_since_boot()
-    self.n_its = self.libmpc.run_mpc(self.cur_state, self.mpc_solution, self.a_lead_tau, a_lead)
-    self.duration = int((sec_since_boot() - t) * 1e9)
+    self.libmpc.run_mpc(self.cur_state, self.mpc_solution,
+                        list(poss), list(speeds), list(accels),
+                        self.min_a, self.max_a)
 
-    # Get solution. MPC timestep is 0.2 s, so interpolation to 0.05 s is needed
-    self.v_mpc = self.mpc_solution[0].v_ego[1]
-    self.a_mpc = self.mpc_solution[0].a_ego[1]
-    self.v_mpc_future = self.mpc_solution[0].v_ego[10]
+    self.v_solution = list(self.mpc_solution.v_ego)
+    self.a_solution = list(self.mpc_solution.a_ego)
 
     # Reset if NaN or goes through lead car
-    crashing = any(lead - ego < -50 for (lead, ego) in zip(self.mpc_solution[0].x_l, self.mpc_solution[0].x_ego))
     nans = any(math.isnan(x) for x in self.mpc_solution[0].v_ego)
-    backwards = min(self.mpc_solution[0].v_ego) < -0.01
 
-    if ((backwards or crashing) and self.prev_lead_status) or nans:
+    t = sec_since_boot()
+    if nans:
       if t > self.last_cloudlog_t + 5.0:
         self.last_cloudlog_t = t
-        cloudlog.warning("Longitudinal mpc %d reset - backwards: %s crashing: %s nan: %s" % (
-                          self.mpc_id, backwards, crashing, nans))
-
-      self.libmpc.init(MPC_COST_LONG.TTC, MPC_COST_LONG.DISTANCE,
-                       MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK)
-      self.cur_state[0].v_ego = v_ego
-      self.cur_state[0].a_ego = 0.0
-      self.v_mpc = v_ego
-      self.a_mpc = CS.aEgo
-      self.prev_lead_status = False
+        cloudlog.warning("Longitudinal model mpc reset - nans")
+      self.reset_mpc()

selfdrive/controls/lib/long_mpc_model.py

@@ -1,73 +0,0 @@
-import numpy as np
-import math
-
-from selfdrive.swaglog import cloudlog
-from common.realtime import sec_since_boot
-from selfdrive.controls.lib.longitudinal_mpc_model import libmpc_py
-
-
-class LongitudinalMpcModel():
-  def __init__(self):
-
-    self.setup_mpc()
-    self.v_mpc = 0.0
-    self.v_mpc_future = 0.0
-    self.a_mpc = 0.0
-    self.last_cloudlog_t = 0.0
-    self.ts = list(range(10))
-
-    self.valid = False
-
-  def setup_mpc(self, v_ego=0.0):
-    self.libmpc = libmpc_py.libmpc
-    self.libmpc.init(1.0, 1.0, 1.0, 1.0, 1.0)
-    self.libmpc.init_with_simulation(v_ego)
-
-    self.mpc_solution = libmpc_py.ffi.new("log_t *")
-    self.cur_state = libmpc_py.ffi.new("state_t *")
-
-    self.cur_state[0].x_ego = 0
-    self.cur_state[0].v_ego = 0
-    self.cur_state[0].a_ego = 0
-
-  def set_cur_state(self, v, a):
-    self.cur_state[0].x_ego = 0.0
-    self.cur_state[0].v_ego = v
-    self.cur_state[0].a_ego = a
-
-  def update(self, v_ego, a_ego, poss, speeds, accels):
-    if len(poss) == 0:
-      self.valid = False
-      return
-
-    x_poly = list(map(float, np.polyfit(self.ts, poss, 3)))
-    v_poly = list(map(float, np.polyfit(self.ts, speeds, 3)))
-    a_poly = list(map(float, np.polyfit(self.ts, accels, 3)))
-
-    # Calculate mpc
-    self.libmpc.run_mpc(self.cur_state, self.mpc_solution, x_poly, v_poly, a_poly)
-
-    # Get solution. MPC timestep is 0.2 s, so interpolation to 0.05 s is needed
-    self.v_mpc = self.mpc_solution[0].v_ego[1]
-    self.a_mpc = self.mpc_solution[0].a_ego[1]
-    self.v_mpc_future = self.mpc_solution[0].v_ego[10]
-    self.valid = True
-
-    # Reset if NaN or goes through lead car
-    nans = any(math.isnan(x) for x in self.mpc_solution[0].v_ego)
-
-    t = sec_since_boot()
-    if nans:
-      if t > self.last_cloudlog_t + 5.0:
-        self.last_cloudlog_t = t
-        cloudlog.warning("Longitudinal model mpc reset - backwards")
-
-      self.libmpc.init(1.0, 1.0, 1.0, 1.0, 1.0)
-      self.libmpc.init_with_simulation(v_ego)
-
-      self.cur_state[0].v_ego = v_ego
-      self.cur_state[0].a_ego = 0.0
-
-      self.v_mpc = v_ego
-      self.a_mpc = a_ego
-      self.valid = False

selfdrive/controls/lib/longcontrol.py

@@ -1,6 +1,8 @@
 from cereal import log
 from common.numpy_fast import clip, interp
 from selfdrive.controls.lib.pid import PIController
+from selfdrive.controls.lib.drive_helpers import CONTROL_N
+from selfdrive.modeld.constants import T_IDXS
 
 LongCtrlState = log.ControlsState.LongControlState
 
@@ -12,6 +14,7 @@ BRAKE_THRESHOLD_TO_PID = 0.2
 BRAKE_STOPPING_TARGET = 0.5  # apply at least this amount of brake to maintain the vehicle stationary
 
 RATE = 100.0
+DEFAULT_LONG_LAG = 0.15
 
 
 def long_control_state_trans(active, long_control_state, v_ego, v_target, v_pid,
@@ -66,8 +69,20 @@ class LongControl():
     self.pid.reset()
     self.v_pid = v_pid
 
-  def update(self, active, CS, v_target, v_target_future, a_target, CP):
+  def update(self, active, CS, CP, long_plan):
     """Update longitudinal control. This updates the state machine and runs a PID loop"""
+    # Interp control trajectory
+    # TODO estimate car specific lag, use .5s for now
+    if len(long_plan.speeds) == CONTROL_N:
+      v_target = interp(DEFAULT_LONG_LAG, T_IDXS[:CONTROL_N], long_plan.speeds)
+      v_target_future = long_plan.speeds[-1]
+      a_target = interp(DEFAULT_LONG_LAG, T_IDXS[:CONTROL_N], long_plan.accels)
+    else:
+      v_target = 0.0
+      v_target_future = 0.0
+      a_target = 0.0
+
+
     # Actuation limits
     gas_max = interp(CS.vEgo, CP.gasMaxBP, CP.gasMaxV)
     brake_max = interp(CS.vEgo, CP.brakeMaxBP, CP.brakeMaxV)
@@ -119,4 +134,4 @@ class LongControl():
     final_gas = clip(output_gb, 0., gas_max)
     final_brake = -clip(output_gb, -brake_max, 0.)
 
-    return final_gas, final_brake
+    return final_gas, final_brake, v_target, a_target

selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_qpoases_interface.hpp

@@ -1,3 +0,0 @@
-version https://git-lfs.github.com/spec/v1
-oid sha256:3cc91e06813e2f18c87f0f2c798eb76a4e81e12c1027892a6c2b7d3451b03d54
-size 1821

selfdrive/controls/lib/longitudinal_mpc_lib/SConscript

@@ -0,0 +1,47 @@
+Import('env', 'arch')
+
+cpp_path = [
+    "#",
+    "#selfdrive",
+    "#phonelibs/acado/include",
+    "#phonelibs/acado/include/acado",
+    "#phonelibs/qpoases/INCLUDE",
+    "#phonelibs/qpoases/INCLUDE/EXTRAS",
+    "#phonelibs/qpoases/SRC/",
+    "#phonelibs/qpoases",
+    "lib_mpc_export",
+]
+
+generated_c = [
+    'lib_mpc_export/acado_auxiliary_functions.c',
+    'lib_mpc_export/acado_qpoases_interface.cpp',
+    'lib_mpc_export/acado_integrator.c',
+    'lib_mpc_export/acado_solver.c',
+]
+
+generated_h = [
+    'lib_mpc_export/acado_common.h',
+    'lib_mpc_export/acado_auxiliary_functions.h',
+    'lib_mpc_export/acado_qpoases_interface.hpp',
+]
+
+interface_dir = Dir('lib_mpc_export')
+
+SConscript(['#phonelibs/qpoases/SConscript'], variant_dir='lib_qp', exports=['interface_dir'])
+
+if GetOption('mpc_generate'):
+  generator_cpp = File('generator.cpp')
+
+  acado_libs = [File(f"#phonelibs/acado/{arch}/lib/libacado_toolkit.a"),
+                File(f"#phonelibs/acado/{arch}/lib/libacado_casadi.a"),
+                File(f"#phonelibs/acado/{arch}/lib/libacado_csparse.a")]
+
+  generator = env.Program('generator', generator_cpp, LIBS=acado_libs, CPPPATH=cpp_path,
+                          CCFLAGS=env['CCFLAGS'] + ["-Wno-deprecated", "-Wno-overloaded-shift-op-parentheses"])
+
+  cmd = f"cd {Dir('.').get_abspath()} && {generator[0].get_abspath()}"
+  env.Command(generated_c + generated_h, generator, cmd)
+
+
+mpc_files = ["longitudinal_mpc.c"] + generated_c
+env.SharedLibrary('mpc', mpc_files, LIBS=['m', 'qpoases'], LIBPATH=['lib_qp'], CPPPATH=cpp_path)

selfdrive/controls/lib/longitudinal_mpc_lib/generator.cpp

@@ -0,0 +1,82 @@
+#include <acado_code_generation.hpp>
+#include "selfdrive/common/modeldata.h"
+
+using namespace std;
+
+
+int main( )
+{
+  USING_NAMESPACE_ACADO
+
+
+  DifferentialEquation f;
+
+  DifferentialState x_ego, v_ego, a_ego;
+  DifferentialState dummy_0;
+  OnlineData min_a, max_a;
+
+  Control j_ego, accel_slack;
+
+  // Equations of motion
+  f << dot(x_ego) == v_ego;
+  f << dot(v_ego) == a_ego;
+  f << dot(a_ego) == j_ego;
+  f << dot(dummy_0) == accel_slack;
+
+  // Running cost
+  Function h;
+  h << x_ego;
+  h << v_ego;
+  h << a_ego;
+  h << j_ego;
+  h << accel_slack;
+
+  // Weights are defined in mpc.
+  BMatrix Q(5,5); Q.setAll(true);
+
+  // Terminal cost
+  Function hN;
+  hN << x_ego;
+  hN << v_ego;
+  hN << a_ego;
+
+  // Weights are defined in mpc.
+  BMatrix QN(3,3); QN.setAll(true);
+
+  double T_IDXS_ARR[LON_MPC_N + 1];
+  memcpy(T_IDXS_ARR, T_IDXS, (LON_MPC_N + 1) * sizeof(double));
+  Grid times(LON_MPC_N + 1, T_IDXS_ARR);
+  OCP ocp(times);
+  ocp.subjectTo(f);
+
+  ocp.minimizeLSQ(Q, h);
+  ocp.minimizeLSQEndTerm(QN, hN);
+
+  ocp.subjectTo( 0.0 <= v_ego);
+  ocp.subjectTo( 0.0 <= a_ego - min_a + accel_slack);
+  ocp.subjectTo( a_ego - max_a + accel_slack <= 0.0);
+  ocp.setNOD(2);
+
+  OCPexport mpc(ocp);
+  mpc.set( HESSIAN_APPROXIMATION, GAUSS_NEWTON );
+  mpc.set( DISCRETIZATION_TYPE, MULTIPLE_SHOOTING );
+  mpc.set( INTEGRATOR_TYPE, INT_RK4 );
+  mpc.set( NUM_INTEGRATOR_STEPS, 1000);
+  mpc.set( MAX_NUM_QP_ITERATIONS, 50);
+  mpc.set( CG_USE_VARIABLE_WEIGHTING_MATRIX, YES);
+
+  mpc.set( SPARSE_QP_SOLUTION, CONDENSING );
+  mpc.set( QP_SOLVER, QP_QPOASES );
+  mpc.set( HOTSTART_QP, YES );
+  mpc.set( GENERATE_TEST_FILE, NO);
+  mpc.set( GENERATE_MAKE_FILE, NO );
+  mpc.set( GENERATE_MATLAB_INTERFACE, NO );
+  mpc.set( GENERATE_SIMULINK_INTERFACE, NO );
+
+  if (mpc.exportCode( "lib_mpc_export" ) != SUCCESSFUL_RETURN)
+    exit( EXIT_FAILURE );
+
+  mpc.printDimensionsQP( );
+
+  return EXIT_SUCCESS;
+}

selfdrive/controls/lib/longitudinal_mpc_lib/lib_mpc_export/acado_auxiliary_functions.c

@@ -0,0 +1,212 @@
+/*
+ *    This file was auto-generated using the ACADO Toolkit.
+ *    
+ *    While ACADO Toolkit is free software released under the terms of
+ *    the GNU Lesser General Public License (LGPL), the generated code
+ *    as such remains the property of the user who used ACADO Toolkit
+ *    to generate this code. In particular, user dependent data of the code
+ *    do not inherit the GNU LGPL license. On the other hand, parts of the
+ *    generated code that are a direct copy of source code from the
+ *    ACADO Toolkit or the software tools it is based on, remain, as derived
+ *    work, automatically covered by the LGPL license.
+ *    
+ *    ACADO Toolkit is distributed in the hope that it will be useful,
+ *    but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ *    
+ */
+
+
+#include "acado_auxiliary_functions.h"
+
+#include <stdio.h>
+
+real_t* acado_getVariablesX( )
+{
+	return acadoVariables.x;
+}
+
+real_t* acado_getVariablesU( )
+{
+	return acadoVariables.u;
+}
+
+#if ACADO_NY > 0
+real_t* acado_getVariablesY( )
+{
+	return acadoVariables.y;
+}
+#endif
+
+#if ACADO_NYN > 0
+real_t* acado_getVariablesYN( )
+{
+	return acadoVariables.yN;
+}
+#endif
+
+real_t* acado_getVariablesX0( )
+{
+#if ACADO_INITIAL_VALUE_FIXED
+	return acadoVariables.x0;
+#else
+	return 0;
+#endif
+}
+
+/** Print differential variables. */
+void acado_printDifferentialVariables( )
+{
+	int i, j;
+	printf("\nDifferential variables:\n[\n");
+	for (i = 0; i < ACADO_N + 1; ++i)
+	{
+		for (j = 0; j < ACADO_NX; ++j)
+			printf("\t%e", acadoVariables.x[i * ACADO_NX + j]);
+		printf("\n");
+	}
+	printf("]\n\n");
+}
+
+/** Print control variables. */
+void acado_printControlVariables( )
+{
+	int i, j;
+	printf("\nControl variables:\n[\n");
+	for (i = 0; i < ACADO_N; ++i)
+	{
+		for (j = 0; j < ACADO_NU; ++j)
+			printf("\t%e", acadoVariables.u[i * ACADO_NU + j]);
+		printf("\n");
+	}
+	printf("]\n\n");
+}
+
+/** Print ACADO code generation notice. */
+void acado_printHeader( )
+{
+	printf(
+		"\nACADO Toolkit -- A Toolkit for Automatic Control and Dynamic Optimization.\n"
+		"Copyright (C) 2008-2015 by Boris Houska, Hans Joachim Ferreau,\n" 
+		"Milan Vukov and Rien Quirynen, KU Leuven.\n"
+	);
+	
+	printf(
+		"Developed within the Optimization in Engineering Center (OPTEC) under\n"
+		"supervision of Moritz Diehl. All rights reserved.\n\n"
+		"ACADO Toolkit is distributed under the terms of the GNU Lesser\n"
+		"General Public License 3 in the hope that it will be useful,\n"
+		"but WITHOUT ANY WARRANTY; without even the implied warranty of\n"
+		"MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the\n"
+		"GNU Lesser General Public License for more details.\n\n"
+	);
+}
+
+#if !(defined _DSPACE)
+#if (defined _WIN32 || defined _WIN64) && !(defined __MINGW32__ || defined __MINGW64__)
+
+void acado_tic( acado_timer* t )
+{
+	QueryPerformanceFrequency(&t->freq);
+	QueryPerformanceCounter(&t->tic);
+}
+
+real_t acado_toc( acado_timer* t )
+{
+	QueryPerformanceCounter(&t->toc);
+	return ((t->toc.QuadPart - t->tic.QuadPart) / (real_t)t->freq.QuadPart);
+}
+
+
+#elif (defined __APPLE__)
+
+void acado_tic( acado_timer* t )
+{
+    /* read current clock cycles */
+    t->tic = mach_absolute_time();
+}
+
+real_t acado_toc( acado_timer* t )
+{
+
+    uint64_t duration; /* elapsed time in clock cycles*/
+    
+    t->toc = mach_absolute_time();
+    duration = t->toc - t->tic;
+    
+    /*conversion from clock cycles to nanoseconds*/
+    mach_timebase_info(&(t->tinfo));
+    duration *= t->tinfo.numer;
+    duration /= t->tinfo.denom;
+
+    return (real_t)duration / 1e9;
+}
+
+#else
+
+#if __STDC_VERSION__ >= 199901L
+/* C99 mode */
+
+/* read current time */
+void acado_tic( acado_timer* t )
+{
+	gettimeofday(&t->tic, 0);
+}
+
+/* return time passed since last call to tic on this timer */
+real_t acado_toc( acado_timer* t )
+{
+	struct timeval temp;
+	
+	gettimeofday(&t->toc, 0);
+    
+	if ((t->toc.tv_usec - t->tic.tv_usec) < 0)
+	{
+		temp.tv_sec = t->toc.tv_sec - t->tic.tv_sec - 1;
+		temp.tv_usec = 1000000 + t->toc.tv_usec - t->tic.tv_usec;
+	}
+	else
+	{
+		temp.tv_sec = t->toc.tv_sec - t->tic.tv_sec;
+		temp.tv_usec = t->toc.tv_usec - t->tic.tv_usec;
+	}
+	
+	return (real_t)temp.tv_sec + (real_t)temp.tv_usec / 1e6;
+}
+
+#else
+/* ANSI */
+
+/* read current time */
+void acado_tic( acado_timer* t )
+{
+	clock_gettime(CLOCK_MONOTONIC, &t->tic);
+}
+
+
+/* return time passed since last call to tic on this timer */
+real_t acado_toc( acado_timer* t )
+{
+	struct timespec temp;
+    
+	clock_gettime(CLOCK_MONOTONIC, &t->toc);	
+    
+	if ((t->toc.tv_nsec - t->tic.tv_nsec) < 0)
+	{
+		temp.tv_sec = t->toc.tv_sec - t->tic.tv_sec - 1;
+		temp.tv_nsec = 1000000000+t->toc.tv_nsec - t->tic.tv_nsec;
+	}
+	else
+	{
+		temp.tv_sec = t->toc.tv_sec - t->tic.tv_sec;
+		temp.tv_nsec = t->toc.tv_nsec - t->tic.tv_nsec;
+	}
+	
+	return (real_t)temp.tv_sec + (real_t)temp.tv_nsec / 1e9;
+}
+
+#endif /* __STDC_VERSION__ >= 199901L */
+
+#endif /* (defined _WIN32 || _WIN64) */
+
+#endif

selfdrive/controls/lib/longitudinal_mpc_lib/lib_mpc_export/acado_auxiliary_functions.h

@@ -0,0 +1,138 @@
+/*
+ *    This file was auto-generated using the ACADO Toolkit.
+ *    
+ *    While ACADO Toolkit is free software released under the terms of
+ *    the GNU Lesser General Public License (LGPL), the generated code
+ *    as such remains the property of the user who used ACADO Toolkit
+ *    to generate this code. In particular, user dependent data of the code
+ *    do not inherit the GNU LGPL license. On the other hand, parts of the
+ *    generated code that are a direct copy of source code from the
+ *    ACADO Toolkit or the software tools it is based on, remain, as derived
+ *    work, automatically covered by the LGPL license.
+ *    
+ *    ACADO Toolkit is distributed in the hope that it will be useful,
+ *    but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ *    
+ */
+
+
+#ifndef ACADO_AUXILIARY_FUNCTIONS_H
+#define ACADO_AUXILIARY_FUNCTIONS_H
+
+#include "acado_common.h"
+
+#ifndef __MATLAB__
+#ifdef __cplusplus
+extern "C"
+{
+#endif /* __cplusplus */
+#endif /* __MATLAB__ */
+
+/** Get pointer to the matrix with differential variables. */
+real_t* acado_getVariablesX( );
+
+/** Get pointer to the matrix with control variables. */
+real_t* acado_getVariablesU( );
+
+#if ACADO_NY > 0
+/** Get pointer to the matrix with references/measurements. */
+real_t* acado_getVariablesY( );
+#endif
+
+#if ACADO_NYN > 0
+/** Get pointer to the vector with references/measurement on the last node. */
+real_t* acado_getVariablesYN( );
+#endif
+
+/** Get pointer to the current state feedback vector. Only applicable for NMPC. */
+real_t* acado_getVariablesX0( );
+
+/** Print differential variables. */
+void acado_printDifferentialVariables( );
+
+/** Print control variables. */
+void acado_printControlVariables( );
+
+/** Print ACADO code generation notice. */
+void acado_printHeader( );
+
+/*
+ * A huge thanks goes to Alexander Domahidi from ETHZ, Switzerland, for 
+ * providing us with the following timing routines.
+ */
+
+#if !(defined _DSPACE)
+#if (defined _WIN32 || defined _WIN64) && !(defined __MINGW32__ || defined __MINGW64__)
+
+/* Use Windows QueryPerformanceCounter for timing. */
+#include <Windows.h>
+
+/** A structure for keeping internal timer data. */
+typedef struct acado_timer_
+{
+	LARGE_INTEGER tic;
+	LARGE_INTEGER toc;
+	LARGE_INTEGER freq;
+} acado_timer;
+
+
+#elif (defined __APPLE__)
+
+#include "unistd.h"
+#include <mach/mach_time.h>
+
+/** A structure for keeping internal timer data. */
+typedef struct acado_timer_
+{
+	uint64_t tic;
+	uint64_t toc;
+	mach_timebase_info_data_t tinfo;
+} acado_timer;
+
+#else
+
+/* Use POSIX clock_gettime() for timing on non-Windows machines. */
+#include <time.h>
+
+#if __STDC_VERSION__ >= 199901L
+/* C99 mode of operation. */
+
+#include <sys/stat.h>
+#include <sys/time.h>
+
+typedef struct acado_timer_
+{
+	struct timeval tic;
+	struct timeval toc;
+} acado_timer;
+
+#else
+/* ANSI C */
+
+/** A structure for keeping internal timer data. */
+typedef struct acado_timer_
+{
+	struct timespec tic;
+	struct timespec toc;
+} acado_timer;
+
+#endif /* __STDC_VERSION__ >= 199901L */
+
+#endif /* (defined _WIN32 || defined _WIN64) */
+
+/** A function for measurement of the current time. */
+void acado_tic( acado_timer* t );
+
+/** A function which returns the elapsed time. */
+real_t acado_toc( acado_timer* t );
+
+#endif
+
+#ifndef __MATLAB__
+#ifdef __cplusplus
+} /* extern "C" */
+#endif /* __cplusplus */
+#endif /* __MATLAB__ */
+
+#endif /* ACADO_AUXILIARY_FUNCTIONS_H */

selfdrive/controls/lib/longitudinal_mpc_lib/lib_mpc_export/acado_common.h

@@ -0,0 +1,372 @@
+/*
+ *    This file was auto-generated using the ACADO Toolkit.
+ *    
+ *    While ACADO Toolkit is free software released under the terms of
+ *    the GNU Lesser General Public License (LGPL), the generated code
+ *    as such remains the property of the user who used ACADO Toolkit
+ *    to generate this code. In particular, user dependent data of the code
+ *    do not inherit the GNU LGPL license. On the other hand, parts of the
+ *    generated code that are a direct copy of source code from the
+ *    ACADO Toolkit or the software tools it is based on, remain, as derived
+ *    work, automatically covered by the LGPL license.
+ *    
+ *    ACADO Toolkit is distributed in the hope that it will be useful,
+ *    but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ *    
+ */
+
+
+#ifndef ACADO_COMMON_H
+#define ACADO_COMMON_H
+
+#include <math.h>
+#include <string.h>
+
+#ifndef __MATLAB__
+#ifdef __cplusplus
+extern "C"
+{
+#endif /* __cplusplus */
+#endif /* __MATLAB__ */
+
+/** \defgroup ACADO ACADO CGT generated module. */
+/** @{ */
+
+/** qpOASES QP solver indicator. */
+#define ACADO_QPOASES  0
+#define ACADO_QPOASES3 1
+/** FORCES QP solver indicator.*/
+#define ACADO_FORCES   2
+/** qpDUNES QP solver indicator.*/
+#define ACADO_QPDUNES  3
+/** HPMPC QP solver indicator. */
+#define ACADO_HPMPC    4
+#define ACADO_GENERIC    5
+
+/** Indicator for determining the QP solver used by the ACADO solver code. */
+#define ACADO_QP_SOLVER ACADO_QPOASES
+
+#include "acado_qpoases_interface.hpp"
+
+
+/*
+ * Common definitions
+ */
+/** User defined block based condensing. */
+#define ACADO_BLOCK_CONDENSING 0
+/** Compute covariance matrix of the last state estimate. */
+#define ACADO_COMPUTE_COVARIANCE_MATRIX 0
+/** Flag indicating whether constraint values are hard-coded or not. */
+#define ACADO_HARDCODED_CONSTRAINT_VALUES 1
+/** Indicator for fixed initial state. */
+#define ACADO_INITIAL_STATE_FIXED 1
+/** Number of control/estimation intervals. */
+#define ACADO_N 32
+/** Number of online data values. */
+#define ACADO_NOD 2
+/** Number of path constraints. */
+#define ACADO_NPAC 2
+/** Number of control variables. */
+#define ACADO_NU 2
+/** Number of differential variables. */
+#define ACADO_NX 4
+/** Number of algebraic variables. */
+#define ACADO_NXA 0
+/** Number of differential derivative variables. */
+#define ACADO_NXD 0
+/** Number of references/measurements per node on the first N nodes. */
+#define ACADO_NY 5
+/** Number of references/measurements on the last (N + 1)st node. */
+#define ACADO_NYN 3
+/** Total number of QP optimization variables. */
+#define ACADO_QP_NV 68
+/** Number of Runge-Kutta stages per integration step. */
+#define ACADO_RK_NSTAGES 4
+/** Providing interface for arrival cost. */
+#define ACADO_USE_ARRIVAL_COST 0
+/** Indicator for usage of non-hard-coded linear terms in the objective. */
+#define ACADO_USE_LINEAR_TERMS 0
+/** Indicator for type of fixed weighting matrices. */
+#define ACADO_WEIGHTING_MATRICES_TYPE 2
+
+
+/*
+ * Globally used structure definitions
+ */
+
+/** The structure containing the user data.
+ * 
+ *  Via this structure the user "communicates" with the solver code.
+ */
+typedef struct ACADOvariables_
+{
+int dummy;
+/** Matrix of size: 33 x 4 (row major format)
+ * 
+ *  Matrix containing 33 differential variable vectors.
+ */
+real_t x[ 132 ];
+
+/** Matrix of size: 32 x 2 (row major format)
+ * 
+ *  Matrix containing 32 control variable vectors.
+ */
+real_t u[ 64 ];
+
+/** Matrix of size: 33 x 2 (row major format)
+ * 
+ *  Matrix containing 33 online data vectors.
+ */
+real_t od[ 66 ];
+
+/** Column vector of size: 160
+ * 
+ *  Matrix containing 32 reference/measurement vectors of size 5 for first 32 nodes.
+ */
+real_t y[ 160 ];
+
+/** Column vector of size: 3
+ * 
+ *  Reference/measurement vector for the 33. node.
+ */
+real_t yN[ 3 ];
+
+/** Matrix of size: 160 x 5 (row major format) */
+real_t W[ 800 ];
+
+/** Matrix of size: 3 x 3 (row major format) */
+real_t WN[ 9 ];
+
+/** Column vector of size: 4
+ * 
+ *  Current state feedback vector.
+ */
+real_t x0[ 4 ];
+
+
+} ACADOvariables;
+
+/** Private workspace used by the auto-generated code.
+ * 
+ *  Data members of this structure are private to the solver.
+ *  In other words, the user code should not modify values of this 
+ *  structure. 
+ */
+typedef struct ACADOworkspace_
+{
+real_t rk_ttt;
+
+/** Row vector of size: 32 */
+real_t rk_xxx[ 32 ];
+
+/** Matrix of size: 4 x 28 (row major format) */
+real_t rk_kkk[ 112 ];
+
+/** Row vector of size: 32 */
+real_t state[ 32 ];
+
+/** Column vector of size: 128 */
+real_t d[ 128 ];
+
+/** Column vector of size: 160 */
+real_t Dy[ 160 ];
+
+/** Column vector of size: 3 */
+real_t DyN[ 3 ];
+
+/** Matrix of size: 128 x 4 (row major format) */
+real_t evGx[ 512 ];
+
+/** Matrix of size: 128 x 2 (row major format) */
+real_t evGu[ 256 ];
+
+/** Row vector of size: 8 */
+real_t objValueIn[ 8 ];
+
+/** Row vector of size: 5 */
+real_t objValueOut[ 5 ];
+
+/** Matrix of size: 128 x 4 (row major format) */
+real_t Q1[ 512 ];
+
+/** Matrix of size: 128 x 5 (row major format) */
+real_t Q2[ 640 ];
+
+/** Matrix of size: 64 x 2 (row major format) */
+real_t R1[ 128 ];
+
+/** Matrix of size: 64 x 5 (row major format) */
+real_t R2[ 320 ];
+
+/** Matrix of size: 128 x 2 (row major format) */
+real_t S1[ 256 ];
+
+/** Matrix of size: 4 x 4 (row major format) */
+real_t QN1[ 16 ];
+
+/** Matrix of size: 4 x 3 (row major format) */
+real_t QN2[ 12 ];
+
+/** Column vector of size: 12 */
+real_t conAuxVar[ 12 ];
+
+/** Row vector of size: 8 */
+real_t conValueIn[ 8 ];
+
+/** Row vector of size: 14 */
+real_t conValueOut[ 14 ];
+
+/** Column vector of size: 64 */
+real_t evH[ 64 ];
+
+/** Matrix of size: 64 x 4 (row major format) */
+real_t evHx[ 256 ];
+
+/** Matrix of size: 64 x 2 (row major format) */
+real_t evHu[ 128 ];
+
+/** Column vector of size: 2 */
+real_t evHxd[ 2 ];
+
+/** Column vector of size: 4 */
+real_t Dx0[ 4 ];
+
+/** Matrix of size: 4 x 4 (row major format) */
+real_t T[ 16 ];
+
+/** Matrix of size: 2112 x 2 (row major format) */
+real_t E[ 4224 ];
+
+/** Matrix of size: 2112 x 2 (row major format) */
+real_t QE[ 4224 ];
+
+/** Matrix of size: 128 x 4 (row major format) */
+real_t QGx[ 512 ];
+
+/** Column vector of size: 128 */
+real_t Qd[ 128 ];
+
+/** Column vector of size: 132 */
+real_t QDy[ 132 ];
+
+/** Matrix of size: 64 x 4 (row major format) */
+real_t H10[ 256 ];
+
+/** Matrix of size: 68 x 68 (row major format) */
+real_t H[ 4624 ];
+
+/** Matrix of size: 96 x 68 (row major format) */
+real_t A[ 6528 ];
+
+/** Column vector of size: 68 */
+real_t g[ 68 ];
+
+/** Column vector of size: 68 */
+real_t lb[ 68 ];
+
+/** Column vector of size: 68 */
+real_t ub[ 68 ];
+
+/** Column vector of size: 96 */
+real_t lbA[ 96 ];
+
+/** Column vector of size: 96 */
+real_t ubA[ 96 ];
+
+/** Column vector of size: 68 */
+real_t x[ 68 ];
+
+/** Column vector of size: 164 */
+real_t y[ 164 ];
+
+
+} ACADOworkspace;
+
+/* 
+ * Forward function declarations. 
+ */
+
+
+/** Performs the integration and sensitivity propagation for one shooting interval.
+ *
+ *  \param rk_eta Working array to pass the input values and return the results.
+ *  \param resetIntegrator The internal memory of the integrator can be reset.
+ *  \param rk_index Number of the shooting interval.
+ *
+ *  \return Status code of the integrator.
+ */
+int acado_integrate( real_t* const rk_eta, int resetIntegrator, int rk_index );
+
+/** Export of an ACADO symbolic function.
+ *
+ *  \param in Input to the exported function.
+ *  \param out Output of the exported function.
+ */
+void acado_rhs_forw(const real_t* in, real_t* out);
+
+/** Preparation step of the RTI scheme.
+ *
+ *  \return Status of the integration module. =0: OK, otherwise the error code.
+ */
+int acado_preparationStep(  );
+
+/** Feedback/estimation step of the RTI scheme.
+ *
+ *  \return Status code of the qpOASES QP solver.
+ */
+int acado_feedbackStep(  );
+
+/** Solver initialization. Must be called once before any other function call.
+ *
+ *  \return =0: OK, otherwise an error code of a QP solver.
+ */
+int acado_initializeSolver(  );
+
+/** Initialize shooting nodes by a forward simulation starting from the first node.
+ */
+void acado_initializeNodesByForwardSimulation(  );
+
+/** Shift differential variables vector by one interval.
+ *
+ *  \param strategy Shifting strategy: 1. Initialize node 33 with xEnd. 2. Initialize node 33 by forward simulation.
+ *  \param xEnd Value for the x vector on the last node. If =0 the old value is used.
+ *  \param uEnd Value for the u vector on the second to last node. If =0 the old value is used.
+ */
+void acado_shiftStates( int strategy, real_t* const xEnd, real_t* const uEnd );
+
+/** Shift controls vector by one interval.
+ *
+ *  \param uEnd Value for the u vector on the second to last node. If =0 the old value is used.
+ */
+void acado_shiftControls( real_t* const uEnd );
+
+/** Get the KKT tolerance of the current iterate.
+ *
+ *  \return The KKT tolerance value.
+ */
+real_t acado_getKKT(  );
+
+/** Calculate the objective value.
+ *
+ *  \return Value of the objective function.
+ */
+real_t acado_getObjective(  );
+
+
+/* 
+ * Extern declarations. 
+ */
+
+extern ACADOworkspace acadoWorkspace;
+extern ACADOvariables acadoVariables;
+
+/** @} */
+
+#ifndef __MATLAB__
+#ifdef __cplusplus
+} /* extern "C" */
+#endif /* __cplusplus */
+#endif /* __MATLAB__ */
+
+#endif /* ACADO_COMMON_H */

selfdrive/controls/lib/longitudinal_mpc_lib/lib_mpc_export/acado_integrator.c

@@ -0,0 +1,250 @@
+/*
+ *    This file was auto-generated using the ACADO Toolkit.
+ *    
+ *    While ACADO Toolkit is free software released under the terms of
+ *    the GNU Lesser General Public License (LGPL), the generated code
+ *    as such remains the property of the user who used ACADO Toolkit
+ *    to generate this code. In particular, user dependent data of the code
+ *    do not inherit the GNU LGPL license. On the other hand, parts of the
+ *    generated code that are a direct copy of source code from the
+ *    ACADO Toolkit or the software tools it is based on, remain, as derived
+ *    work, automatically covered by the LGPL license.
+ *    
+ *    ACADO Toolkit is distributed in the hope that it will be useful,
+ *    but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ *    
+ */
+
+
+#include "acado_common.h"
+
+
+void acado_rhs_forw(const real_t* in, real_t* out)
+{
+const real_t* xd = in;
+const real_t* u = in + 28;
+
+/* Compute outputs: */
+out[0] = xd[1];
+out[1] = xd[2];
+out[2] = u[0];
+out[3] = u[1];
+out[4] = xd[8];
+out[5] = xd[9];
+out[6] = xd[10];
+out[7] = xd[11];
+out[8] = xd[12];
+out[9] = xd[13];
+out[10] = xd[14];
+out[11] = xd[15];
+out[12] = (real_t)(0.0000000000000000e+00);
+out[13] = (real_t)(0.0000000000000000e+00);
+out[14] = (real_t)(0.0000000000000000e+00);
+out[15] = (real_t)(0.0000000000000000e+00);
+out[16] = (real_t)(0.0000000000000000e+00);
+out[17] = (real_t)(0.0000000000000000e+00);
+out[18] = (real_t)(0.0000000000000000e+00);
+out[19] = (real_t)(0.0000000000000000e+00);
+out[20] = xd[22];
+out[21] = xd[23];
+out[22] = xd[24];
+out[23] = xd[25];
+out[24] = (real_t)(1.0000000000000000e+00);
+out[25] = (real_t)(0.0000000000000000e+00);
+out[26] = (real_t)(0.0000000000000000e+00);
+out[27] = (real_t)(1.0000000000000000e+00);
+}
+
+/* Fixed step size:0.01 */
+int acado_integrate( real_t* const rk_eta, int resetIntegrator, int rk_index )
+{
+int error;
+
+int run1;
+int numSteps[32] = {1, 3, 5, 7, 9, 11, 13, 15, 17, 19, 21, 22, 24, 26, 28, 30, 32, 34, 36, 38, 40, 42, 44, 46, 48, 50, 52, 54, 56, 58, 60, 62};
+int numInts = numSteps[rk_index];
+acadoWorkspace.rk_ttt = 0.0000000000000000e+00;
+rk_eta[4] = 1.0000000000000000e+00;
+rk_eta[5] = 0.0000000000000000e+00;
+rk_eta[6] = 0.0000000000000000e+00;
+rk_eta[7] = 0.0000000000000000e+00;
+rk_eta[8] = 0.0000000000000000e+00;
+rk_eta[9] = 1.0000000000000000e+00;
+rk_eta[10] = 0.0000000000000000e+00;
+rk_eta[11] = 0.0000000000000000e+00;
+rk_eta[12] = 0.0000000000000000e+00;
+rk_eta[13] = 0.0000000000000000e+00;
+rk_eta[14] = 1.0000000000000000e+00;
+rk_eta[15] = 0.0000000000000000e+00;
+rk_eta[16] = 0.0000000000000000e+00;
+rk_eta[17] = 0.0000000000000000e+00;
+rk_eta[18] = 0.0000000000000000e+00;
+rk_eta[19] = 1.0000000000000000e+00;
+rk_eta[20] = 0.0000000000000000e+00;
+rk_eta[21] = 0.0000000000000000e+00;
+rk_eta[22] = 0.0000000000000000e+00;
+rk_eta[23] = 0.0000000000000000e+00;
+rk_eta[24] = 0.0000000000000000e+00;
+rk_eta[25] = 0.0000000000000000e+00;
+rk_eta[26] = 0.0000000000000000e+00;
+rk_eta[27] = 0.0000000000000000e+00;
+acadoWorkspace.rk_xxx[28] = rk_eta[28];
+acadoWorkspace.rk_xxx[29] = rk_eta[29];
+acadoWorkspace.rk_xxx[30] = rk_eta[30];
+acadoWorkspace.rk_xxx[31] = rk_eta[31];
+
+for (run1 = 0; run1 < 1; ++run1)
+{
+for(run1 = 0; run1 < numInts; run1++ ) {
+acadoWorkspace.rk_xxx[0] = + rk_eta[0];
+acadoWorkspace.rk_xxx[1] = + rk_eta[1];
+acadoWorkspace.rk_xxx[2] = + rk_eta[2];
+acadoWorkspace.rk_xxx[3] = + rk_eta[3];
+acadoWorkspace.rk_xxx[4] = + rk_eta[4];
+acadoWorkspace.rk_xxx[5] = + rk_eta[5];
+acadoWorkspace.rk_xxx[6] = + rk_eta[6];
+acadoWorkspace.rk_xxx[7] = + rk_eta[7];
+acadoWorkspace.rk_xxx[8] = + rk_eta[8];
+acadoWorkspace.rk_xxx[9] = + rk_eta[9];
+acadoWorkspace.rk_xxx[10] = + rk_eta[10];
+acadoWorkspace.rk_xxx[11] = + rk_eta[11];
+acadoWorkspace.rk_xxx[12] = + rk_eta[12];
+acadoWorkspace.rk_xxx[13] = + rk_eta[13];
+acadoWorkspace.rk_xxx[14] = + rk_eta[14];
+acadoWorkspace.rk_xxx[15] = + rk_eta[15];
+acadoWorkspace.rk_xxx[16] = + rk_eta[16];
+acadoWorkspace.rk_xxx[17] = + rk_eta[17];
+acadoWorkspace.rk_xxx[18] = + rk_eta[18];
+acadoWorkspace.rk_xxx[19] = + rk_eta[19];
+acadoWorkspace.rk_xxx[20] = + rk_eta[20];
+acadoWorkspace.rk_xxx[21] = + rk_eta[21];
+acadoWorkspace.rk_xxx[22] = + rk_eta[22];
+acadoWorkspace.rk_xxx[23] = + rk_eta[23];
+acadoWorkspace.rk_xxx[24] = + rk_eta[24];
+acadoWorkspace.rk_xxx[25] = + rk_eta[25];
+acadoWorkspace.rk_xxx[26] = + rk_eta[26];
+acadoWorkspace.rk_xxx[27] = + rk_eta[27];
+acado_rhs_forw( acadoWorkspace.rk_xxx, acadoWorkspace.rk_kkk );
+acadoWorkspace.rk_xxx[0] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[0] + rk_eta[0];
+acadoWorkspace.rk_xxx[1] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[1] + rk_eta[1];
+acadoWorkspace.rk_xxx[2] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[2] + rk_eta[2];
+acadoWorkspace.rk_xxx[3] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[3] + rk_eta[3];
+acadoWorkspace.rk_xxx[4] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[4] + rk_eta[4];
+acadoWorkspace.rk_xxx[5] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[5] + rk_eta[5];
+acadoWorkspace.rk_xxx[6] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[6] + rk_eta[6];
+acadoWorkspace.rk_xxx[7] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[7] + rk_eta[7];
+acadoWorkspace.rk_xxx[8] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[8] + rk_eta[8];
+acadoWorkspace.rk_xxx[9] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[9] + rk_eta[9];
+acadoWorkspace.rk_xxx[10] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[10] + rk_eta[10];
+acadoWorkspace.rk_xxx[11] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[11] + rk_eta[11];
+acadoWorkspace.rk_xxx[12] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[12] + rk_eta[12];
+acadoWorkspace.rk_xxx[13] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[13] + rk_eta[13];
+acadoWorkspace.rk_xxx[14] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[14] + rk_eta[14];
+acadoWorkspace.rk_xxx[15] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[15] + rk_eta[15];
+acadoWorkspace.rk_xxx[16] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[16] + rk_eta[16];
+acadoWorkspace.rk_xxx[17] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[17] + rk_eta[17];
+acadoWorkspace.rk_xxx[18] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[18] + rk_eta[18];
+acadoWorkspace.rk_xxx[19] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[19] + rk_eta[19];
+acadoWorkspace.rk_xxx[20] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[20] + rk_eta[20];
+acadoWorkspace.rk_xxx[21] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[21] + rk_eta[21];
+acadoWorkspace.rk_xxx[22] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[22] + rk_eta[22];
+acadoWorkspace.rk_xxx[23] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[23] + rk_eta[23];
+acadoWorkspace.rk_xxx[24] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[24] + rk_eta[24];
+acadoWorkspace.rk_xxx[25] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[25] + rk_eta[25];
+acadoWorkspace.rk_xxx[26] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[26] + rk_eta[26];
+acadoWorkspace.rk_xxx[27] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[27] + rk_eta[27];
+acado_rhs_forw( acadoWorkspace.rk_xxx, &(acadoWorkspace.rk_kkk[ 28 ]) );
+acadoWorkspace.rk_xxx[0] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[28] + rk_eta[0];
+acadoWorkspace.rk_xxx[1] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[29] + rk_eta[1];
+acadoWorkspace.rk_xxx[2] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[30] + rk_eta[2];
+acadoWorkspace.rk_xxx[3] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[31] + rk_eta[3];
+acadoWorkspace.rk_xxx[4] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[32] + rk_eta[4];
+acadoWorkspace.rk_xxx[5] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[33] + rk_eta[5];
+acadoWorkspace.rk_xxx[6] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[34] + rk_eta[6];
+acadoWorkspace.rk_xxx[7] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[35] + rk_eta[7];
+acadoWorkspace.rk_xxx[8] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[36] + rk_eta[8];
+acadoWorkspace.rk_xxx[9] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[37] + rk_eta[9];
+acadoWorkspace.rk_xxx[10] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[38] + rk_eta[10];
+acadoWorkspace.rk_xxx[11] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[39] + rk_eta[11];
+acadoWorkspace.rk_xxx[12] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[40] + rk_eta[12];
+acadoWorkspace.rk_xxx[13] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[41] + rk_eta[13];
+acadoWorkspace.rk_xxx[14] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[42] + rk_eta[14];
+acadoWorkspace.rk_xxx[15] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[43] + rk_eta[15];
+acadoWorkspace.rk_xxx[16] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[44] + rk_eta[16];
+acadoWorkspace.rk_xxx[17] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[45] + rk_eta[17];
+acadoWorkspace.rk_xxx[18] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[46] + rk_eta[18];
+acadoWorkspace.rk_xxx[19] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[47] + rk_eta[19];
+acadoWorkspace.rk_xxx[20] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[48] + rk_eta[20];
+acadoWorkspace.rk_xxx[21] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[49] + rk_eta[21];
+acadoWorkspace.rk_xxx[22] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[50] + rk_eta[22];
+acadoWorkspace.rk_xxx[23] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[51] + rk_eta[23];
+acadoWorkspace.rk_xxx[24] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[52] + rk_eta[24];
+acadoWorkspace.rk_xxx[25] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[53] + rk_eta[25];
+acadoWorkspace.rk_xxx[26] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[54] + rk_eta[26];
+acadoWorkspace.rk_xxx[27] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[55] + rk_eta[27];
+acado_rhs_forw( acadoWorkspace.rk_xxx, &(acadoWorkspace.rk_kkk[ 56 ]) );
+acadoWorkspace.rk_xxx[0] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[56] + rk_eta[0];
+acadoWorkspace.rk_xxx[1] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[57] + rk_eta[1];
+acadoWorkspace.rk_xxx[2] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[58] + rk_eta[2];
+acadoWorkspace.rk_xxx[3] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[59] + rk_eta[3];
+acadoWorkspace.rk_xxx[4] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[60] + rk_eta[4];
+acadoWorkspace.rk_xxx[5] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[61] + rk_eta[5];
+acadoWorkspace.rk_xxx[6] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[62] + rk_eta[6];
+acadoWorkspace.rk_xxx[7] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[63] + rk_eta[7];
+acadoWorkspace.rk_xxx[8] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[64] + rk_eta[8];
+acadoWorkspace.rk_xxx[9] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[65] + rk_eta[9];
+acadoWorkspace.rk_xxx[10] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[66] + rk_eta[10];
+acadoWorkspace.rk_xxx[11] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[67] + rk_eta[11];
+acadoWorkspace.rk_xxx[12] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[68] + rk_eta[12];
+acadoWorkspace.rk_xxx[13] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[69] + rk_eta[13];
+acadoWorkspace.rk_xxx[14] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[70] + rk_eta[14];
+acadoWorkspace.rk_xxx[15] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[71] + rk_eta[15];
+acadoWorkspace.rk_xxx[16] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[72] + rk_eta[16];
+acadoWorkspace.rk_xxx[17] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[73] + rk_eta[17];
+acadoWorkspace.rk_xxx[18] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[74] + rk_eta[18];
+acadoWorkspace.rk_xxx[19] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[75] + rk_eta[19];
+acadoWorkspace.rk_xxx[20] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[76] + rk_eta[20];
+acadoWorkspace.rk_xxx[21] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[77] + rk_eta[21];
+acadoWorkspace.rk_xxx[22] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[78] + rk_eta[22];
+acadoWorkspace.rk_xxx[23] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[79] + rk_eta[23];
+acadoWorkspace.rk_xxx[24] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[80] + rk_eta[24];
+acadoWorkspace.rk_xxx[25] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[81] + rk_eta[25];
+acadoWorkspace.rk_xxx[26] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[82] + rk_eta[26];
+acadoWorkspace.rk_xxx[27] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[83] + rk_eta[27];
+acado_rhs_forw( acadoWorkspace.rk_xxx, &(acadoWorkspace.rk_kkk[ 84 ]) );
+rk_eta[0] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[0] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[28] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[56] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[84];
+rk_eta[1] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[1] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[29] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[57] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[85];
+rk_eta[2] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[2] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[30] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[58] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[86];
+rk_eta[3] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[3] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[31] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[59] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[87];
+rk_eta[4] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[4] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[32] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[60] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[88];
+rk_eta[5] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[5] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[33] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[61] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[89];
+rk_eta[6] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[6] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[34] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[62] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[90];
+rk_eta[7] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[7] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[35] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[63] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[91];
+rk_eta[8] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[8] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[36] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[64] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[92];
+rk_eta[9] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[9] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[37] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[65] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[93];
+rk_eta[10] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[10] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[38] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[66] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[94];
+rk_eta[11] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[11] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[39] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[67] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[95];
+rk_eta[12] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[12] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[40] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[68] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[96];
+rk_eta[13] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[13] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[41] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[69] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[97];
+rk_eta[14] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[14] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[42] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[70] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[98];
+rk_eta[15] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[15] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[43] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[71] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[99];
+rk_eta[16] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[16] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[44] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[72] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[100];
+rk_eta[17] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[17] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[45] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[73] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[101];
+rk_eta[18] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[18] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[46] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[74] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[102];
+rk_eta[19] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[19] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[47] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[75] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[103];
+rk_eta[20] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[20] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[48] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[76] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[104];
+rk_eta[21] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[21] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[49] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[77] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[105];
+rk_eta[22] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[22] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[50] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[78] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[106];
+rk_eta[23] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[23] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[51] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[79] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[107];
+rk_eta[24] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[24] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[52] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[80] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[108];
+rk_eta[25] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[25] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[53] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[81] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[109];
+rk_eta[26] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[26] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[54] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[82] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[110];
+rk_eta[27] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[27] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[55] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[83] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[111];
+acadoWorkspace.rk_ttt += 1.0000000000000000e+00;
+}
+}
+error = 0;
+return error;
+}
+

selfdrive/controls/lib/longitudinal_mpc_lib/lib_mpc_export/acado_qpoases_interface.cpp

@@ -0,0 +1,70 @@
+/*
+ *    This file was auto-generated using the ACADO Toolkit.
+ *    
+ *    While ACADO Toolkit is free software released under the terms of
+ *    the GNU Lesser General Public License (LGPL), the generated code
+ *    as such remains the property of the user who used ACADO Toolkit
+ *    to generate this code. In particular, user dependent data of the code
+ *    do not inherit the GNU LGPL license. On the other hand, parts of the
+ *    generated code that are a direct copy of source code from the
+ *    ACADO Toolkit or the software tools it is based on, remain, as derived
+ *    work, automatically covered by the LGPL license.
+ *    
+ *    ACADO Toolkit is distributed in the hope that it will be useful,
+ *    but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ *    
+ */
+
+
+extern "C"
+{
+#include "acado_common.h"
+}
+
+#include "INCLUDE/QProblem.hpp"
+
+#if ACADO_COMPUTE_COVARIANCE_MATRIX == 1
+#include "INCLUDE/EXTRAS/SolutionAnalysis.hpp"
+#endif /* ACADO_COMPUTE_COVARIANCE_MATRIX */
+
+static int acado_nWSR;
+
+
+
+#if ACADO_COMPUTE_COVARIANCE_MATRIX == 1
+static SolutionAnalysis acado_sa;
+#endif /* ACADO_COMPUTE_COVARIANCE_MATRIX */
+
+int acado_solve( void )
+{
+	acado_nWSR = QPOASES_NWSRMAX;
+
+	QProblem qp(68, 96);
+	
+	returnValue retVal = qp.init(acadoWorkspace.H, acadoWorkspace.g, acadoWorkspace.A, acadoWorkspace.lb, acadoWorkspace.ub, acadoWorkspace.lbA, acadoWorkspace.ubA, acado_nWSR, acadoWorkspace.y);
+
+    qp.getPrimalSolution( acadoWorkspace.x );
+    qp.getDualSolution( acadoWorkspace.y );
+	
+#if ACADO_COMPUTE_COVARIANCE_MATRIX == 1
+
+	if (retVal != SUCCESSFUL_RETURN)
+		return (int)retVal;
+		
+	retVal = acado_sa.getHessianInverse( &qp,var );
+
+#endif /* ACADO_COMPUTE_COVARIANCE_MATRIX */
+
+	return (int)retVal;
+}
+
+int acado_getNWSR( void )
+{
+	return acado_nWSR;
+}
+
+const char* acado_getErrorString( int error )
+{
+	return MessageHandling::getErrorString( error );
+}

selfdrive/controls/lib/longitudinal_mpc_lib/lib_mpc_export/acado_qpoases_interface.hpp

@@ -0,0 +1,65 @@
+/*
+ *    This file was auto-generated using the ACADO Toolkit.
+ *    
+ *    While ACADO Toolkit is free software released under the terms of
+ *    the GNU Lesser General Public License (LGPL), the generated code
+ *    as such remains the property of the user who used ACADO Toolkit
+ *    to generate this code. In particular, user dependent data of the code
+ *    do not inherit the GNU LGPL license. On the other hand, parts of the
+ *    generated code that are a direct copy of source code from the
+ *    ACADO Toolkit or the software tools it is based on, remain, as derived
+ *    work, automatically covered by the LGPL license.
+ *    
+ *    ACADO Toolkit is distributed in the hope that it will be useful,
+ *    but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ *    
+ */
+
+
+#ifndef QPOASES_HEADER
+#define QPOASES_HEADER
+
+#ifdef PC_DEBUG
+#include <stdio.h>
+#endif /* PC_DEBUG */
+
+#include <math.h>
+
+#ifdef __cplusplus
+#define EXTERNC extern "C"
+#else
+#define EXTERNC
+#endif
+
+/*
+ * A set of options for qpOASES
+ */
+
+/** Maximum number of optimization variables. */
+#define QPOASES_NVMAX      68
+/** Maximum number of constraints. */
+#define QPOASES_NCMAX      96
+/** Maximum number of working set recalculations. */
+#define QPOASES_NWSRMAX    50
+/** Print level for qpOASES. */
+#define QPOASES_PRINTLEVEL PL_NONE
+/** The value of EPS */
+#define QPOASES_EPS        2.221e-16
+/** Internally used floating point type */
+typedef double real_t;
+
+/*
+ * Forward function declarations
+ */
+
+/** A function that calls the QP solver */
+EXTERNC int acado_solve( void );
+
+/** Get the number of active set changes */
+EXTERNC int acado_getNWSR( void );
+
+/** Get the error string. */
+const char* acado_getErrorString( int error );
+
+#endif /* QPOASES_HEADER */

selfdrive/controls/lib/longitudinal_mpc_lib/libmpc_py.py

@@ -0,0 +1,34 @@
+import os
+
+from cffi import FFI
+from common.ffi_wrapper import suffix
+
+mpc_dir = os.path.join(os.path.dirname(os.path.abspath(__file__)))
+libmpc_fn = os.path.join(mpc_dir, "libmpc"+suffix())
+
+ffi = FFI()
+ffi.cdef("""
+const int MPC_N = 32;
+
+typedef struct {
+double x_ego, v_ego, a_ego;
+} state_t;
+
+
+typedef struct {
+double x_ego[MPC_N+1];
+double v_ego[MPC_N+1];
+double a_ego[MPC_N+1];
+double t[MPC_N+1];
+double j_ego[MPC_N];
+double cost;
+} log_t;
+
+
+void init(double xCost, double vCost, double aCost, double jerkCost, double constraintCost);
+int run_mpc(state_t * x0, log_t * solution,
+            double target_x[MPC_N+1], double target_v[MPC_N+1], double target_a[MPC_N+1],
+            double min_a, double max_a);
+""")
+
+libmpc = ffi.dlopen(libmpc_fn)

selfdrive/controls/lib/longitudinal_mpc_lib/longitudinal_mpc.c

@@ -1,5 +1,6 @@
 #include "acado_common.h"
 #include "acado_auxiliary_functions.h"
+#include "common/modeldata.h"
 
 #include <stdio.h>
 #include <math.h>
@@ -31,7 +32,7 @@ typedef struct {
   double cost;
 } log_t;
 
-void init(double xCost, double vCost, double aCost, double accelCost, double jerkCost) {
+void init(double xCost, double vCost, double aCost, double jerkCost, double constraintCost){
   acado_initializeSolver();
   int    i;
   const int STEP_MULTIPLIER = 3;
@@ -46,79 +47,46 @@ void init(double xCost, double vCost, double aCost, double accelCost, double jer
 
   /* MPC: initialize the current state feedback. */
   for (i = 0; i < NX; ++i) acadoVariables.x0[ i ] = 0.0;
+  
   // Set weights
-
   for (i = 0; i < N; i++) {
-    int f = 1;
-    if (i > 4) {
-      f = STEP_MULTIPLIER;
-    }
+    double f = 20 * (T_IDXS[i+1] - T_IDXS[i]);
     // Setup diagonal entries
     acadoVariables.W[NY*NY*i + (NY+1)*0] = xCost * f;
     acadoVariables.W[NY*NY*i + (NY+1)*1] = vCost * f;
     acadoVariables.W[NY*NY*i + (NY+1)*2] = aCost * f;
-    acadoVariables.W[NY*NY*i + (NY+1)*3] = accelCost * f;
-    acadoVariables.W[NY*NY*i + (NY+1)*4] = jerkCost * f;
+    acadoVariables.W[NY*NY*i + (NY+1)*3] = jerkCost * f;
+    acadoVariables.W[NY*NY*i + (NY+1)*4] = constraintCost * f;
   }
   acadoVariables.WN[(NYN+1)*0] = xCost * STEP_MULTIPLIER;
   acadoVariables.WN[(NYN+1)*1] = vCost * STEP_MULTIPLIER;
   acadoVariables.WN[(NYN+1)*2] = aCost * STEP_MULTIPLIER;
-  acadoVariables.WN[(NYN+1)*3] = accelCost * STEP_MULTIPLIER;
 
 }
 
-void init_with_simulation(double v_ego) {
-  int i;
-
-  double x_ego = 0.0;
-
-  double dt = 0.2;
-  double t = 0.0;
-
-  for (i = 0; i < N + 1; ++i) {
-    if (i > 4) {
-      dt = 0.6;
-    }
-
-    acadoVariables.x[i*NX] = x_ego;
-    acadoVariables.x[i*NX+1] = v_ego;
-    acadoVariables.x[i*NX+2] = 0;
-    acadoVariables.x[i*NX+3] = t;
-
-    x_ego += v_ego * dt;
-    t += dt;
-  }
-
-  for (i = 0; i < NU * N; ++i)  acadoVariables.u[ i ] = 0.0;
-  for (i = 0; i < NY * N; ++i)  acadoVariables.y[ i ] = 0.0;
-  for (i = 0; i < NYN; ++i)  acadoVariables.yN[ i ] = 0.0;
-}
 
 int run_mpc(state_t * x0, log_t * solution,
-            double x_poly[4], double v_poly[4], double a_poly[4]) {
+            double target_x[N+1], double target_v[N+1], double target_a[N+1],
+            double min_a, double max_a){
   int i;
-
-  for (i = 0; i < N + 1; ++i) {
-    acadoVariables.od[i*NOD+0] = x_poly[0];
-    acadoVariables.od[i*NOD+1] = x_poly[1];
-    acadoVariables.od[i*NOD+2] = x_poly[2];
-    acadoVariables.od[i*NOD+3] = x_poly[3];
-
-    acadoVariables.od[i*NOD+4] = v_poly[0];
-    acadoVariables.od[i*NOD+5] = v_poly[1];
-    acadoVariables.od[i*NOD+6] = v_poly[2];
-    acadoVariables.od[i*NOD+7] = v_poly[3];
-
-    acadoVariables.od[i*NOD+8] = a_poly[0];
-    acadoVariables.od[i*NOD+9] = a_poly[1];
-    acadoVariables.od[i*NOD+10] = a_poly[2];
-    acadoVariables.od[i*NOD+11] = a_poly[3];
+  for (i = 0; i < N + 1; ++i){
+    acadoVariables.od[i*NOD] = min_a;
+    acadoVariables.od[i*NOD+1] = max_a;
+  }
+  for (i = 0; i < N; i+= 1){
+    acadoVariables.y[NY*i + 0] = target_x[i];
+    acadoVariables.y[NY*i + 1] = target_v[i];
+    acadoVariables.y[NY*i + 2] = target_a[i];
+    acadoVariables.y[NY*i + 3] = 0.0;
+    acadoVariables.y[NY*i + 4] = 0.0;
   }
+  acadoVariables.yN[0] = target_x[N];
+  acadoVariables.yN[1] = target_v[N];
+  acadoVariables.yN[2] = target_a[N];
 
-  acadoVariables.x[0] = acadoVariables.x0[0] = x0->x_ego;
-  acadoVariables.x[1] = acadoVariables.x0[1] = x0->v_ego;
-  acadoVariables.x[2] = acadoVariables.x0[2] = x0->a_ego;
-  acadoVariables.x[3] = acadoVariables.x0[3] = 0;
+  acadoVariables.x0[0] = x0->x_ego;
+  acadoVariables.x0[1] = x0->v_ego;
+  acadoVariables.x0[2] = x0->a_ego;
 
   acado_preparationStep();
   acado_feedbackStep();
@@ -127,10 +95,9 @@ int run_mpc(state_t * x0, log_t * solution,
     solution->x_ego[i] = acadoVariables.x[i*NX];
     solution->v_ego[i] = acadoVariables.x[i*NX+1];
     solution->a_ego[i] = acadoVariables.x[i*NX+2];
-    solution->t[i] = acadoVariables.x[i*NX+3];
 
     if (i < N) {
-      solution->j_ego[i] = acadoVariables.u[i];
+      solution->j_ego[i] = acadoVariables.u[i*NU];
     }
   }
   solution->cost = acado_getObjective();

selfdrive/controls/lib/longitudinal_mpc_model/SConscript

@@ -1,31 +0,0 @@
-Import('env', 'arch')
-
-
-cpp_path = [
-    "#phonelibs/acado/include",
-    "#phonelibs/acado/include/acado",
-    "#phonelibs/qpoases/INCLUDE",
-    "#phonelibs/qpoases/INCLUDE/EXTRAS",
-    "#phonelibs/qpoases/SRC/",
-    "#phonelibs/qpoases",
-    "lib_mpc_export"
-
-]
-
-mpc_files = [
-    "longitudinal_mpc.c",
-    Glob("lib_mpc_export/*.c"),
-    Glob("lib_mpc_export/*.cpp"),
-]
-
-interface_dir = Dir('lib_mpc_export')
-
-SConscript(['#phonelibs/qpoases/SConscript'], variant_dir='lib_qp', exports=['interface_dir'])
-
-env.SharedLibrary('mpc', mpc_files, LIBS=['m', 'qpoases'], LIBPATH=['lib_qp'], CPPPATH=cpp_path)
-
-# if arch != "aarch64":
-#     acado_libs = [File("#phonelibs/acado/x64/lib/libacado_toolkit.a"),
-#                   File("#phonelibs/acado/x64/lib/libacado_casadi.a"),
-#                   File("#phonelibs/acado/x64/lib/libacado_csparse.a")]
-#     env.Program('generator', 'generator.cpp', LIBS=acado_libs, CPPPATH=cpp_path)

selfdrive/controls/lib/longitudinal_mpc_model/generator.cpp

@@ -1,99 +0,0 @@
-#include <acado_code_generation.hpp>
-
-const int controlHorizon = 50;
-
-using namespace std;
-
-
-int main( )
-{
-  USING_NAMESPACE_ACADO
-
-
-  DifferentialEquation f;
-
-  DifferentialState x_ego, v_ego, a_ego, t;
-
-  OnlineData x_poly_r0, x_poly_r1, x_poly_r2, x_poly_r3;
-  OnlineData v_poly_r0, v_poly_r1, v_poly_r2, v_poly_r3;
-  OnlineData a_poly_r0, a_poly_r1, a_poly_r2, a_poly_r3;
-
-  Control j_ego;
-
-  // Equations of motion
-  f << dot(x_ego) == v_ego;
-  f << dot(v_ego) == a_ego;
-  f << dot(a_ego) == j_ego;
-  f << dot(t) == 1;
-
-  auto poly_x = x_poly_r0*(t*t*t) + x_poly_r1*(t*t) + x_poly_r2*t + x_poly_r3;
-  auto poly_v = v_poly_r0*(t*t*t) + v_poly_r1*(t*t) + v_poly_r2*t + v_poly_r3;
-  auto poly_a = a_poly_r0*(t*t*t) + a_poly_r1*(t*t) + a_poly_r2*t + a_poly_r3;
-
-  // Running cost
-  Function h;
-  h << x_ego - poly_x;
-  h << v_ego - poly_v;
-  h << a_ego - poly_a;
-  h << a_ego * (0.1 * v_ego + 1.0);
-  h << j_ego * (0.1 * v_ego + 1.0);
-
-  // Weights are defined in mpc.
-  BMatrix Q(5,5); Q.setAll(true);
-
-  // Terminal cost
-  Function hN;
-  hN << x_ego - poly_x;
-  hN << v_ego - poly_v;
-  hN << a_ego - poly_a;
-  hN << a_ego * (0.1 * v_ego + 1.0);
-
-  // Weights are defined in mpc.
-  BMatrix QN(4,4); QN.setAll(true);
-
-  // Non uniform time grid
-  // First 5 timesteps are 0.2, after that it's 0.6
-  DMatrix numSteps(20, 1);
-  for (int i = 0; i < 5; i++){
-    numSteps(i) = 1;
-  }
-  for (int i = 5; i < 20; i++){
-    numSteps(i) = 3;
-  }
-
-  // Setup Optimal Control Problem
-  const double tStart = 0.0;
-  const double tEnd   = 10.0;
-
-  OCP ocp( tStart, tEnd, numSteps);
-  ocp.subjectTo(f);
-
-  ocp.minimizeLSQ(Q, h);
-  ocp.minimizeLSQEndTerm(QN, hN);
-
-  //ocp.subjectTo( 0.0 <= v_ego);
-  ocp.setNOD(12);
-
-  OCPexport mpc(ocp);
-  mpc.set( HESSIAN_APPROXIMATION, GAUSS_NEWTON );
-  mpc.set( DISCRETIZATION_TYPE, MULTIPLE_SHOOTING );
-  mpc.set( INTEGRATOR_TYPE, INT_RK4 );
-  mpc.set( NUM_INTEGRATOR_STEPS, controlHorizon);
-  mpc.set( MAX_NUM_QP_ITERATIONS, 500);
-  mpc.set( CG_USE_VARIABLE_WEIGHTING_MATRIX, YES);
-
-  mpc.set( SPARSE_QP_SOLUTION, CONDENSING );
-  mpc.set( QP_SOLVER, QP_QPOASES );
-  mpc.set( HOTSTART_QP, YES );
-  mpc.set( GENERATE_TEST_FILE, NO);
-  mpc.set( GENERATE_MAKE_FILE, NO );
-  mpc.set( GENERATE_MATLAB_INTERFACE, NO );
-  mpc.set( GENERATE_SIMULINK_INTERFACE, NO );
-
-  if (mpc.exportCode( "lib_mpc_export" ) != SUCCESSFUL_RETURN)
-    exit( EXIT_FAILURE );
-
-  mpc.printDimensionsQP( );
-
-  return EXIT_SUCCESS;
-}

selfdrive/controls/lib/longitudinal_mpc_model/lib_mpc_export/acado_auxiliary_functions.c

@@ -1,3 +0,0 @@
-version https://git-lfs.github.com/spec/v1
-oid sha256:36c26a2590e54135f7f03b8c784b434d2bd5ef0d42e7e2a9022c2bb56d0e2357
-size 4906

selfdrive/controls/lib/longitudinal_mpc_model/lib_mpc_export/acado_auxiliary_functions.h

@@ -1,3 +0,0 @@
-version https://git-lfs.github.com/spec/v1
-oid sha256:606244b9b31362cc30c324793191d9bd34a098d5ebf526612749f437a75a4270
-size 3428

selfdrive/controls/lib/longitudinal_mpc_model/lib_mpc_export/acado_common.h

@@ -1,3 +0,0 @@
-version https://git-lfs.github.com/spec/v1
-oid sha256:75e4db5112c976591459850ff64e6765078c541e19bbbb7d57d7ecab478cf002
-size 8537

selfdrive/controls/lib/longitudinal_mpc_model/lib_mpc_export/acado_integrator.c

@@ -1,3 +0,0 @@
-version https://git-lfs.github.com/spec/v1
-oid sha256:efee44ba07ee0b17daa1c672ad1ab36adc64cb4b3ce4994a99620593f8841f31
-size 17893

selfdrive/controls/lib/longitudinal_mpc_model/lib_mpc_export/acado_qpoases_interface.cpp

@@ -1,3 +0,0 @@
-version https://git-lfs.github.com/spec/v1
-oid sha256:4648c886b327c86b9e59da337272087a5a31c716e8bf9023b71ae55036bfbc82
-size 1890

selfdrive/controls/lib/longitudinal_mpc_model/lib_mpc_export/acado_qpoases_interface.hpp

@@ -1,3 +0,0 @@
-version https://git-lfs.github.com/spec/v1
-oid sha256:0d8783b453fbc75097fcb0f287c89a503ce7f858c0c84e3a19a0c581dd559055
-size 1820

selfdrive/controls/lib/longitudinal_mpc_model/lib_mpc_export/acado_solver.c

@@ -1,3 +0,0 @@
-version https://git-lfs.github.com/spec/v1
-oid sha256:4985f68cc7d1fc1e587477faa2fd0ca4ebc9ece598f8c2d20e94555d7a51805a
-size 375557

selfdrive/controls/lib/longitudinal_mpc_model/libmpc_py.py

@@ -1,32 +0,0 @@
-import os
-
-from cffi import FFI
-from common.ffi_wrapper import suffix
-
-mpc_dir = os.path.join(os.path.dirname(os.path.abspath(__file__)))
-libmpc_fn = os.path.join(mpc_dir, "libmpc"+suffix())
-
-ffi = FFI()
-ffi.cdef("""
-
-typedef struct {
-double x_ego, v_ego, a_ego;
-} state_t;
-
-
-typedef struct {
-double x_ego[21];
-double v_ego[21];
-double a_ego[21];
-double t[21];
-double j_ego[20];
-double cost;
-} log_t;
-
-
-void init(double xCost, double vCost, double aCost, double accelCost, double jerkCost);
-void init_with_simulation(double v_ego);
-int run_mpc(state_t * x0, log_t * solution, double x_poly[4], double v_poly[4], double a_poly[4]);
-""")
-
-libmpc = ffi.dlopen(libmpc_fn)

selfdrive/controls/lib/longitudinal_planner.py

@@ -1,48 +1,31 @@
 #!/usr/bin/env python3
 import math
 import numpy as np
-from common.params import Params
 from common.numpy_fast import interp
 
 import cereal.messaging as messaging
+from cereal import log
+from common.realtime import DT_MDL
 from common.realtime import sec_since_boot
-from selfdrive.swaglog import cloudlog
+from selfdrive.modeld.constants import T_IDXS
 from selfdrive.config import Conversions as CV
-from selfdrive.controls.lib.speed_smoother import speed_smoother
-from selfdrive.controls.lib.longcontrol import LongCtrlState
 from selfdrive.controls.lib.fcw import FCWChecker
+from selfdrive.controls.lib.longcontrol import LongCtrlState
+from selfdrive.controls.lib.lead_mpc import LeadMpc
 from selfdrive.controls.lib.long_mpc import LongitudinalMpc
-from selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX
+from selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX, CONTROL_N
+from selfdrive.swaglog import cloudlog
 
 LON_MPC_STEP = 0.2  # first step is 0.2s
 AWARENESS_DECEL = -0.2     # car smoothly decel at .2m/s^2 when user is distracted
-
-# lookup tables VS speed to determine min and max accels in cruise
-# make sure these accelerations are smaller than mpc limits
-_A_CRUISE_MIN_V = [-1.0, -.8, -.67, -.5, -.30]
-_A_CRUISE_MIN_BP = [  0.,  5.,  10., 20.,  40.]
-
-# need fast accel at very low speed for stop and go
-# make sure these accelerations are smaller than mpc limits
-_A_CRUISE_MAX_V = [1.2, 1.2, 0.65, .4]
-_A_CRUISE_MAX_V_FOLLOWING = [1.6, 1.6, 0.65, .4]
-_A_CRUISE_MAX_BP = [0.,  6.4, 22.5, 40.]
+A_CRUISE_MIN = -1.2
+A_CRUISE_MAX = 1.2
 
 # Lookup table for turns
 _A_TOTAL_MAX_V = [1.7, 3.2]
 _A_TOTAL_MAX_BP = [20., 40.]
 
 
-def calc_cruise_accel_limits(v_ego, following):
-  a_cruise_min = interp(v_ego, _A_CRUISE_MIN_BP, _A_CRUISE_MIN_V)
-
-  if following:
-    a_cruise_max = interp(v_ego, _A_CRUISE_MAX_BP, _A_CRUISE_MAX_V_FOLLOWING)
-  else:
-    a_cruise_max = interp(v_ego, _A_CRUISE_MAX_BP, _A_CRUISE_MAX_V)
-  return np.vstack([a_cruise_min, a_cruise_max])
-
-
 def limit_accel_in_turns(v_ego, angle_steers, a_target, CP):
   """
   This function returns a limited long acceleration allowed, depending on the existing lateral acceleration
@@ -59,159 +42,102 @@ def limit_accel_in_turns(v_ego, angle_steers, a_target, CP):
 class Planner():
   def __init__(self, CP):
     self.CP = CP
+    self.mpcs = {}
+    self.mpcs['lead0'] = LeadMpc(0)
+    self.mpcs['lead1'] = LeadMpc(1)
+    self.mpcs['cruise'] = LongitudinalMpc()
 
-    self.mpc1 = LongitudinalMpc(1)
-    self.mpc2 = LongitudinalMpc(2)
-
-    self.v_acc_start = 0.0
-    self.a_acc_start = 0.0
-    self.v_acc_next = 0.0
-    self.a_acc_next = 0.0
-
-    self.v_acc = 0.0
-    self.v_acc_future = 0.0
-    self.a_acc = 0.0
-    self.v_cruise = 0.0
-    self.a_cruise = 0.0
+    self.fcw = False
+    self.fcw_checker = FCWChecker()
 
+    self.v_desired = 0.0
+    self.a_desired = 0.0
     self.longitudinalPlanSource = 'cruise'
-    self.fcw_checker = FCWChecker()
-    self.path_x = np.arange(192)
+    self.alpha = np.exp(-DT_MDL/2.0)
+    self.lead_0 = log.ModelDataV2.LeadDataV3.new_message()
+    self.lead_1 = log.ModelDataV2.LeadDataV3.new_message()
 
-    self.fcw = False
+    self.v_desired_trajectory = np.zeros(CONTROL_N)
+    self.a_desired_trajectory = np.zeros(CONTROL_N)
 
-    self.params = Params()
-    self.first_loop = True
-
-  def choose_solution(self, v_cruise_setpoint, enabled):
-    if enabled:
-      solutions = {'cruise': self.v_cruise}
-      if self.mpc1.prev_lead_status:
-        solutions['mpc1'] = self.mpc1.v_mpc
-      if self.mpc2.prev_lead_status:
-        solutions['mpc2'] = self.mpc2.v_mpc
-
-      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
-        self.a_acc = self.mpc1.a_mpc
-      elif slowest == 'mpc2':
-        self.v_acc = self.mpc2.v_mpc
-        self.a_acc = self.mpc2.a_mpc
-      elif slowest == 'cruise':
-        self.v_acc = self.v_cruise
-        self.a_acc = self.a_cruise
-
-    self.v_acc_future = min([self.mpc1.v_mpc_future, self.mpc2.v_mpc_future, v_cruise_setpoint])
 
   def update(self, sm, CP):
-    """Gets called when new radarState is available"""
     cur_time = sec_since_boot()
     v_ego = sm['carState'].vEgo
+    a_ego = sm['carState'].aEgo
 
-    long_control_state = sm['controlsState'].longControlState
     v_cruise_kph = sm['controlsState'].vCruise
-    force_slow_decel = sm['controlsState'].forceDecel
-
     v_cruise_kph = min(v_cruise_kph, V_CRUISE_MAX)
-    v_cruise_setpoint = v_cruise_kph * CV.KPH_TO_MS
+    v_cruise = v_cruise_kph * CV.KPH_TO_MS
+
+    long_control_state = sm['controlsState'].longControlState
+    force_slow_decel = sm['controlsState'].forceDecel
 
-    lead_1 = sm['radarState'].leadOne
-    lead_2 = sm['radarState'].leadTwo
+    self.lead_0 = sm['radarState'].leadOne
+    self.lead_1 = sm['radarState'].leadTwo
 
     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
-
-    self.v_acc_start = self.v_acc_next
-    self.a_acc_start = self.a_acc_next
-
-    # Calculate speed for normal cruise control
-    if enabled and not self.first_loop and not sm['carState'].gasPressed:
-      accel_limits = [float(x) for x in calc_cruise_accel_limits(v_ego, following)]
-      jerk_limits = [min(-0.1, accel_limits[0]), max(0.1, accel_limits[1])]  # TODO: make a separate lookup for jerk tuning
-      accel_limits_turns = limit_accel_in_turns(v_ego, sm['carState'].steeringAngleDeg, accel_limits, self.CP)
-
-      if force_slow_decel:
-        # if required so, force a smooth deceleration
-        accel_limits_turns[1] = min(accel_limits_turns[1], AWARENESS_DECEL)
-        accel_limits_turns[0] = min(accel_limits_turns[0], accel_limits_turns[1])
-
-      self.v_cruise, self.a_cruise = speed_smoother(self.v_acc_start, self.a_acc_start,
-                                                    v_cruise_setpoint,
-                                                    accel_limits_turns[1], accel_limits_turns[0],
-                                                    jerk_limits[1], jerk_limits[0],
-                                                    LON_MPC_STEP)
-
-      # cruise speed can't be negative even is user is distracted
-      self.v_cruise = max(self.v_cruise, 0.)
-    else:
-      starting = long_control_state == LongCtrlState.starting
-      a_ego = min(sm['carState'].aEgo, 0.0)
-      reset_speed = self.CP.minSpeedCan if starting else v_ego
-      reset_accel = self.CP.startAccel if starting else a_ego
-      self.v_acc = reset_speed
-      self.a_acc = reset_accel
-      self.v_acc_start = reset_speed
-      self.a_acc_start = reset_accel
-      self.v_cruise = reset_speed
-      self.a_cruise = reset_accel
-
-    self.mpc1.set_cur_state(self.v_acc_start, self.a_acc_start)
-    self.mpc2.set_cur_state(self.v_acc_start, self.a_acc_start)
-
-    self.mpc1.update(sm['carState'], lead_1)
-    self.mpc2.update(sm['carState'], lead_2)
-
-    self.choose_solution(v_cruise_setpoint, enabled)
+    if not enabled or sm['carState'].gasPressed:
+      self.v_desired = v_ego
+      self.a_desired = a_ego
+
+    # Prevent divergence, smooth in current v_ego
+    self.v_desired = self.alpha * self.v_desired + (1 - self.alpha) * v_ego
+    self.v_desired = max(0.0, self.v_desired)
+
+    accel_limits = [A_CRUISE_MIN, A_CRUISE_MAX]
+    accel_limits_turns = limit_accel_in_turns(v_ego, sm['carState'].steeringAngleDeg, accel_limits, self.CP)
+    if force_slow_decel:
+      # if required so, force a smooth deceleration
+      accel_limits_turns[1] = min(accel_limits_turns[1], AWARENESS_DECEL)
+      accel_limits_turns[0] = min(accel_limits_turns[0], accel_limits_turns[1])
+    # clip limits, cannot init MPC outside of bounds
+    accel_limits_turns[0] = min(accel_limits_turns[0], self.a_desired)
+    accel_limits_turns[1] = max(accel_limits_turns[1], self.a_desired)
+    self.mpcs['cruise'].set_accel_limits(accel_limits_turns[0], accel_limits_turns[1])
+
+    next_a = np.inf
+    for key in self.mpcs:
+      self.mpcs[key].set_cur_state(self.v_desired, self.a_desired)
+      self.mpcs[key].update(sm['carState'], sm['radarState'], v_cruise)
+      if self.mpcs[key].status and self.mpcs[key].a_solution[5] < next_a:
+        self.longitudinalPlanSource = key
+        self.v_desired_trajectory = self.mpcs[key].v_solution[:CONTROL_N]
+        self.a_desired_trajectory = self.mpcs[key].a_solution[:CONTROL_N]
+        next_a = self.mpcs[key].a_solution[5]
 
     # determine fcw
-    if self.mpc1.new_lead:
+    if self.mpcs['lead0'].new_lead:
       self.fcw_checker.reset_lead(cur_time)
-
     blinkers = sm['carState'].leftBlinker or sm['carState'].rightBlinker
-    self.fcw = self.fcw_checker.update(self.mpc1.mpc_solution, cur_time,
+    self.fcw = self.fcw_checker.update(self.mpcs['lead0'].mpc_solution, cur_time,
                                        sm['controlsState'].active,
                                        v_ego, sm['carState'].aEgo,
-                                       lead_1.dRel, lead_1.vLead, lead_1.aLeadK,
-                                       lead_1.yRel, lead_1.vLat,
-                                       lead_1.fcw, blinkers) and not sm['carState'].brakePressed
+                                       self.lead_1.dRel, self.lead_1.vLead, self.lead_1.aLeadK,
+                                       self.lead_1.yRel, self.lead_1.vLat,
+                                       self.lead_1.fcw, blinkers) and not sm['carState'].brakePressed
     if self.fcw:
       cloudlog.info("FCW triggered %s", self.fcw_checker.counters)
 
     # Interpolate 0.05 seconds and save as starting point for next iteration
-    a_acc_sol = self.a_acc_start + (CP.radarTimeStep / LON_MPC_STEP) * (self.a_acc - self.a_acc_start)
-    v_acc_sol = self.v_acc_start + CP.radarTimeStep * (a_acc_sol + self.a_acc_start) / 2.0
-    self.v_acc_next = v_acc_sol
-    self.a_acc_next = a_acc_sol
-
-    self.first_loop = False
+    a_prev = self.a_desired
+    self.a_desired = np.interp(DT_MDL, T_IDXS[:CONTROL_N], self.a_desired_trajectory)
+    self.v_desired = self.v_desired + DT_MDL * (self.a_desired + a_prev)/2.0
 
   def publish(self, sm, pm):
-    self.mpc1.publish(pm)
-    self.mpc2.publish(pm)
-
     plan_send = messaging.new_message('longitudinalPlan')
 
-    plan_send.valid = sm.all_alive_and_valid(service_list=['carState', 'controlsState', 'radarState'])
+    plan_send.valid = sm.all_alive_and_valid(service_list=['carState', 'controlsState'])
 
     longitudinalPlan = plan_send.longitudinalPlan
-    longitudinalPlan.mdMonoTime = sm.logMonoTime['modelV2']
-    longitudinalPlan.radarStateMonoTime = sm.logMonoTime['radarState']
-
-    longitudinalPlan.vCruise = float(self.v_cruise)
-    longitudinalPlan.aCruise = float(self.a_cruise)
-    longitudinalPlan.vStart = float(self.v_acc_start)
-    longitudinalPlan.aStart = float(self.a_acc_start)
-    longitudinalPlan.vTarget = float(self.v_acc)
-    longitudinalPlan.aTarget = float(self.a_acc)
-    longitudinalPlan.vTargetFuture = float(self.v_acc_future)
-    longitudinalPlan.hasLead = self.mpc1.prev_lead_status
+    longitudinalPlan.modelMonoTime = sm.logMonoTime['modelV2']
+    longitudinalPlan.processingDelay = (plan_send.logMonoTime / 1e9) - sm.logMonoTime['modelV2']
+
+    longitudinalPlan.speeds = [float(x) for x in self.v_desired_trajectory]
+    longitudinalPlan.accels = [float(x) for x in self.a_desired_trajectory]
+
+    longitudinalPlan.hasLead = self.mpcs['lead0'].status
     longitudinalPlan.longitudinalPlanSource = self.longitudinalPlanSource
     longitudinalPlan.fcw = self.fcw
 
-    longitudinalPlan.processingDelay = (plan_send.logMonoTime / 1e9) - sm.rcv_time['radarState']
-
     pm.send('longitudinalPlan', plan_send)

selfdrive/controls/lib/speed_smoother.py

@@ -1,99 +0,0 @@
-import numpy as np
-
-
-def get_delta_out_limits(aEgo, aMax, aMin, jMax, jMin):
-
-  tDelta = 0.
-  if aEgo > aMax:
-    tDelta = (aMax - aEgo) / jMin
-  elif aEgo < aMin:
-    tDelta = (aMin - aEgo) / jMax
-
-  return tDelta
-
-
-def speed_smoother(vEgo, aEgo, vT, aMax, aMin, jMax, jMin, ts):
-
-  dV = vT - vEgo
-
-  # recover quickly if dV is positive and aEgo is negative or viceversa
-  if dV > 0. and aEgo < 0.:
-    jMax *= 3.
-  elif dV < 0. and aEgo > 0.:
-    jMin *= 3.
-
-  tDelta = get_delta_out_limits(aEgo, aMax, aMin, jMax, jMin)
-
-  if (ts <= tDelta):
-    if (aEgo < aMin):
-      vEgo += ts * aEgo + 0.5 * ts**2 * jMax
-      aEgo += ts * jMax
-      return vEgo, aEgo
-    elif (aEgo > aMax):
-      vEgo += ts * aEgo + 0.5 * ts**2 * jMin
-      aEgo += ts * jMin
-      return vEgo, aEgo
-
-  if aEgo > aMax:
-    dV -= 0.5 * (aMax**2 - aEgo**2) / jMin
-    vEgo += 0.5 * (aMax**2 - aEgo**2) / jMin
-    aEgo += tDelta * jMin
-  elif aEgo < aMin:
-    dV -= 0.5 * (aMin**2 - aEgo**2) / jMax
-    vEgo += 0.5 * (aMin**2 - aEgo**2) / jMax
-    aEgo += tDelta * jMax
-
-  ts -= tDelta
-
-  jLim = jMin if aEgo >= 0 else jMax
-  # if we reduce the accel to zero immediately, how much delta speed we generate?
-  dv_min_shift = - 0.5 * aEgo**2 / jLim
-
-  # flip signs so we can consider only one case
-  flipped = False
-  if dV < dv_min_shift:
-    flipped = True
-    dV *= -1
-    vEgo *= -1
-    aEgo *= -1
-    aMax = -aMin
-    jMaxcopy = -jMin
-    jMin = -jMax
-    jMax = jMaxcopy
-
-  # small addition needed to avoid numerical issues with sqrt of ~zero
-  aPeak = np.sqrt((0.5 * aEgo**2 / jMax + dV + 1e-9) / (0.5 / jMax - 0.5 / jMin))
-
-  if aPeak > aMax:
-    aPeak = aMax
-    t1 = (aPeak - aEgo) / jMax
-    if aPeak <= 0:  # there is no solution, so stop after t1
-      t2 = t1 + ts + 1e-9
-      t3 = t2
-    else:
-      vChange = dV - 0.5 * (aPeak**2 - aEgo**2) / jMax + 0.5 * aPeak**2 / jMin
-      if vChange < aPeak * ts:
-        t2 = t1 + vChange / aPeak
-      else:
-        t2 = t1 + ts
-      t3 = t2 - aPeak / jMin
-  else:
-    t1 = (aPeak - aEgo) / jMax
-    t2 = t1
-    t3 = t2 - aPeak / jMin
-
-  dt1 = min(ts, t1)
-  dt2 = max(min(ts, t2) - t1, 0.)
-  dt3 = max(min(ts, t3) - t2, 0.)
-
-  if ts > t3:
-    vEgo += dV
-    aEgo = 0.
-  else:
-    vEgo += aEgo * dt1 + 0.5 * dt1**2 * jMax + aPeak * dt2 + aPeak * dt3 + 0.5 * dt3**2 * jMin
-    aEgo += jMax * dt1 + dt3 * jMin
-
-  vEgo *= -1 if flipped else 1
-  aEgo *= -1 if flipped else 1
-
-  return float(vEgo), float(aEgo)

selfdrive/controls/tests/test_following_distance.py

@@ -1,12 +1,11 @@
+#!/usr/bin/env python3
 import unittest
 import numpy as np
 
 from cereal import log
 import cereal.messaging as messaging
 from selfdrive.config import Conversions as CV
-from selfdrive.controls.lib.longitudinal_planner import calc_cruise_accel_limits
-from selfdrive.controls.lib.speed_smoother import speed_smoother
-from selfdrive.controls.lib.long_mpc import LongitudinalMpc
+from selfdrive.controls.lib.lead_mpc import LeadMpc
 
 
 def RW(v_ego, v_l):
@@ -30,47 +29,35 @@ def run_following_distance_simulation(v_lead, t_end=200.0):
   v_ego = v_lead
   a_ego = 0.0
 
-  v_cruise_setpoint = v_lead + 10.
-
-  pm = FakePubMaster()
-  mpc = LongitudinalMpc(1)
+  mpc = LeadMpc(0)
 
   first = True
   while t < t_end:
-    # Run cruise control
-    accel_limits = [float(x) for x in calc_cruise_accel_limits(v_ego, False)]
-    jerk_limits = [min(-0.1, accel_limits[0]), max(0.1, accel_limits[1])]
-    v_cruise, a_cruise = speed_smoother(v_ego, a_ego, v_cruise_setpoint,
-                                        accel_limits[1], accel_limits[0],
-                                        jerk_limits[1], jerk_limits[0],
-                                        dt)
 
     # Setup CarState
     CS = messaging.new_message('carState')
     CS.carState.vEgo = v_ego
     CS.carState.aEgo = a_ego
 
-    # Setup lead packet
+    # Setup model packet
+    radarstate = messaging.new_message('radarState')
     lead = log.RadarState.LeadData.new_message()
-    lead.status = True
+    lead.modelProb = .75
     lead.dRel = x_lead - x_ego
     lead.vLead = v_lead
     lead.aLeadK = 0.0
+    lead.status = True
+    radarstate.radarState.leadOne = lead
 
     # Run MPC
     mpc.set_cur_state(v_ego, a_ego)
     if first:  # Make sure MPC is converged on first timestep
       for _ in range(20):
-        mpc.update(CS.carState, lead)
-        mpc.publish(pm)
-    mpc.update(CS.carState, lead)
-    mpc.publish(pm)
+        mpc.update(CS.carState, radarstate.radarState, 0)
+    mpc.update(CS.carState, radarstate.radarState, 0)
 
     # Choose slowest of two solutions
-    if v_cruise < mpc.v_mpc:
-      v_ego, a_ego = v_cruise, a_cruise
-    else:
-      v_ego, a_ego = mpc.v_mpc, mpc.a_mpc
+    v_ego, a_ego = mpc.mpc_solution.v_ego[5], mpc.mpc_solution.a_ego[5]
 
     # Update state
     x_lead += v_lead * dt
@@ -89,4 +76,8 @@ class TestFollowingDistance(unittest.TestCase):
       simulation_steady_state = run_following_distance_simulation(v_lead)
       correct_steady_state = RW(v_lead, v_lead) + 4.0
 
-      self.assertAlmostEqual(simulation_steady_state, correct_steady_state, delta=0.1)
+      self.assertAlmostEqual(simulation_steady_state, correct_steady_state, delta=.1)
+
+
+if __name__ == "__main__":
+  unittest.main()

selfdrive/test/longitudinal_maneuvers/plant.py

@@ -86,12 +86,10 @@ class Plant():
       time.sleep(0.01)
       if self.sm.updated['longitudinalPlan']:
         plan = self.sm['longitudinalPlan']
-        self.acceleration = plan.aTarget
+        self.speed = plan.speeds[5]
+        self.acceleration = plan.accels[5]
         fcw = plan.fcw
         break
-    self.speed += self.ts*self.acceleration
-
-
     self.distance_lead = self.distance_lead + v_lead * self.ts
 
     # ******** run the car ********

selfdrive/test/process_replay/ref_commit

@@ -1 +1 @@
-abe59dfbc06ed070a3ac2f4ab587d311ef808e2e
+30a27425ede01b64382326a0d1e96ac80ebeae41

selfdrive/test/test_onroad.py

@@ -154,7 +154,8 @@ class TestOnroad(unittest.TestCase):
     self.assertTrue(cpu_ok)
 
   def test_model_timings(self):
-    cfgs = [("modelV2", 0.035, 0.03), ("driverState", 0.022, 0.020)]
+    #TODO this went up when plannerd cpu usage increased, why?
+    cfgs = [("modelV2", 0.035, 0.03), ("driverState", 0.025, 0.021)]
     for (s, instant_max, avg_max) in cfgs:
       ts = [getattr(getattr(m, s), "modelExecutionTime") for m in self.lr if m.which() == s]
       self.assertLess(min(ts), instant_max, f"high '{s}' execution time: {min(ts)}")