openpilot v0.3.9 release

README.md

@@ -30,21 +30,25 @@ Supported Cars
   - Can only be enabled above 25 mph
 
 - Toyota RAV-4 2016+ with TSS-P (alpha!)
-  - Can only be enabled above 20 mph
+  - By default it uses stock Toyota ACC for longitudinal control
+  - openpilot longitudinal control available after unplugging the Driving Support ECU and can be enabled above 20 mph
+
+- Toyota Prius 2017 (alpha!)
+  - By default it uses stock Toyota ACC for longitudinal control
+  - openpilot longitudinal control available after unplugging the Driving Support ECU
 
 In Progress Cars
 ------
+- Probably all TSS-P Toyota with Steering Assist.
+  - 'Full Speed Range Dynamic Radar Cruise Control' is required to enable stop-and-go. Only the Prius, Camry and C-HR have this option.
+  - Even though the Tundra, Sequoia and the Land Cruiser have TSS-P, they don't have Steering Assist and are not supported.
 
-- Toyota Prius 2017
-
-- Probably all TSS-P Toyota
-
-Community Ported Cars
+Community WIP Cars
 ------
 
-- Chevy Volt 2016-2018 Premier with Driver Confidence II
+- [Chevy Volt 2016-2018 Premier with Driver Confidence II](https://github.com/commaai/openpilot/pull/104)
 
-- Classic Tesla Model S (pre-AP)
+- [Classic Tesla Model S (pre-AP)](https://github.com/commaai/openpilot/pull/145)
 
 Directory structure
 ------
@@ -98,7 +102,7 @@ User Data / chffr Account / Crash Reporting
 
 By default openpilot creates an account and includes a client for chffr, our dashcam app. We use your data to train better models and improve openpilot for everyone.
 
-It's open source software, so you are free to disable it if you wish. 
+It's open source software, so you are free to disable it if you wish.
 
 It logs the road facing camera, CAN, GPS, IMU, magnetometer, thermal sensors, crashes, and operating system logs.
 It does not log the user facing camera or the microphone.

RELEASES.md

@@ -1,3 +1,10 @@
+Version 0.3.9 (2017-11-21)
+==========================
+ * Add alpha support for 2017 Toyota Prius
+ * Improved longitudinal control using model predictive control
+ * Enable Forward Collision Warning
+ * Acura ILX now maintains openpilot engaged at standstill when brakes are applied
+
 Version 0.3.8.2 (2017-10-30)
 ==========================
  * Add alpha support for 2017 Toyota RAV4
@@ -17,9 +24,9 @@ Version 0.3.7 (2017-09-30)
  * Fixed sporadic longitudinal pulsing in Civic
  * Cleanups to vehicle interface
 
-Version 0.3.6.1 (2017-08-15)   
-============================    
- * Mitigate low speed steering oscillations on some vehicles    
+Version 0.3.6.1 (2017-08-15)
+============================
+ * Mitigate low speed steering oscillations on some vehicles
  * Include board steering check for CR-V
 
 Version 0.3.6 (2017-08-08)
@@ -139,4 +146,3 @@ Version 0.1  (2016-11-29)
   * Lane keep assist is working
   * Support for Acura ILX 2016 with AcuraWatch Plus
   * Support for Honda Civic 2016 Touring Edition
-

cereal/car.capnp

@@ -77,6 +77,7 @@ struct CarState {
   # brake pedal, 0.0-1.0
   brake @5 :Float32;      # this is user pedal only
   brakePressed @6 :Bool;  # this is user pedal only
+  brakeLights @19 :Bool;
 
   # steering wheel
   steeringAngle @7 :Float32;   # deg
@@ -92,6 +93,8 @@ struct CarState {
 
   # button presses
   buttonEvents @11 :List(ButtonEvent);
+  leftBlinker @20 :Bool;
+  rightBlinker @21 :Bool;
 
   # which packets this state came from
   canMonoTimes @12: List(UInt64);
@@ -109,6 +112,7 @@ struct CarState {
     speed @1 :Float32;
     available @2 :Bool;
     speedOffset @3 :Float32;
+    standstill @4 :Bool;
   }
 
   enum GearShifter {
@@ -291,10 +295,16 @@ struct CarParams {
   steerKi @17 :Float32;
   steerKf @26 :Float32;
 
+  # Kp and Ki for the longitudinal control
+  longitudinalKpBP @37 :List(Float32);
+  longitudinalKpV @38 :List(Float32);
+  longitudinalKiBP @39 :List(Float32);
+  longitudinalKiV @40 :List(Float32);
+
   steerLimitAlert @30 :Bool;
 
   vEgoStopping @31 :Float32; # Speed at which the car goes into stopping state
   directAccelControl @32 :Bool; # Does the car have direct accel control or just gas/brake
-
-  # TODO: Kp and Ki for long control, perhaps not needed?
+  stoppingControl @35 :Bool; # Does the car allows full control even at lows speeds when stopping
+  startAccel @36 :Float32; # Required acceleraton to overcome creep braking
 }

cereal/log.capnp

@@ -337,12 +337,15 @@ struct Live100Data {
   vTargetLead @3 :Float32;
   upAccelCmd @4 :Float32;
   uiAccelCmd @5 :Float32;
+  ufAccelCmd @33 :Float32;
   yActualDEPRECATED @6 :Float32;
-  yDes @7 :Float32;
+  yDesDEPRECATED @7 :Float32;
   upSteer @8 :Float32;
   uiSteer @9 :Float32;
-  aTargetMin @10 :Float32;
-  aTargetMax @11 :Float32;
+  ufSteer @34 :Float32;
+  aTargetMinDEPRECATED @10 :Float32;
+  aTargetMaxDEPRECATED @11 :Float32;
+  aTarget @35 :Float32;
   jerkFactor @12 :Float32;
   angleSteers @13 :Float32;     # Steering angle in degrees.
   angleSteersDes @29 :Float32;
@@ -350,6 +353,7 @@ struct Live100Data {
   cumLagMs @15 :Float32;
 
   enabled @19 :Bool;
+  active @36 :Bool;
   steerOverride @20 :Bool;
 
   vCruise @22 :Float32;
@@ -388,7 +392,7 @@ struct ModelData {
   leftLane @2 :PathData;
   rightLane @3 :PathData;
   lead @4 :LeadData;
-  leadNew @6 :List(Float32);
+  freePath @6 :List(Float32);
 
   settings @5 :ModelSettings;
 
@@ -472,10 +476,13 @@ struct Plan {
 
   # longitudinal
   longitudinalValid @2 :Bool;
+  vCruise @16 :Float32;
+  aCruise @17 :Float32;
   vTarget @3 :Float32;
   vTargetFuture @14 :Float32;
-  aTargetMin @4 :Float32;
-  aTargetMax @5 :Float32;
+  aTargetMinDEPRECATED @4 :Float32;
+  aTargetMaxDEPRECATED @5 :Float32;
+  aTarget @18 :Float32;
   jerkFactor @6 :Float32;
   hasLead @7 :Bool;
   fcw @8 :Bool;

common/dbc.py

@@ -143,7 +143,7 @@ class dbc(object):
     msg = self.msgs.get(x[0])
     if msg is None:
       if x[0] not in self._warned_addresses:
-        print("WARNING: Unknown message address {}".format(x[0]))
+        #print("WARNING: Unknown message address {}".format(x[0]))
         self._warned_addresses.add(x[0])
       return None, None
 

common/fingerprints.py

@@ -19,6 +19,9 @@ _FINGERPRINTS = {
   "TOYOTA RAV4 2017": {
     36L: 8, 37L: 8, 170L: 8, 180L: 8, 186L: 4, 426L: 6, 452L: 8, 464L: 8, 466L: 8, 467L: 8, 547L: 8, 548L: 8, 552L: 4, 562L: 4, 608L: 8, 610L: 5, 643L: 7, 705L: 8, 725L: 2, 740L: 5, 800L: 8, 835L: 8, 836L: 8, 849L: 4, 869L: 7, 870L: 7, 871L: 2, 896L: 8, 897L: 8, 900L: 6, 902L: 6, 905L: 8, 911L: 8, 916L: 3, 918L: 7, 921L: 8, 933L: 8, 944L: 8, 945L: 8, 951L: 8, 955L: 4, 956L: 8, 979L: 2, 998L: 5, 999L: 7, 1000L: 8, 1001L: 8, 1008L: 2, 1014L: 8, 1017L: 8, 1041L: 8, 1042L: 8, 1043L: 8, 1044L: 8, 1056L: 8, 1059L: 1, 1114L: 8, 1161L: 8, 1162L: 8, 1163L: 8, 1176L: 8, 1177L: 8, 1178L: 8, 1179L: 8, 1180L: 8, 1181L: 8, 1190L: 8, 1191L: 8, 1192L: 8, 1196L: 8, 1227L: 8, 1228L: 8, 1235L: 8, 1237L: 8, 1263L: 8, 1279L: 8, 1408L: 8, 1409L: 8, 1410L: 8, 1552L: 8, 1553L: 8, 1554L: 8, 1555L: 8, 1556L: 8, 1557L: 8, 1561L: 8, 1562L: 8, 1568L: 8, 1569L: 8, 1570L: 8, 1571L: 8, 1572L: 8, 1584L: 8, 1589L: 8, 1592L: 8, 1593L: 8, 1595L: 8, 1596L: 8, 1597L: 8, 1600L: 8, 1656L: 8, 1664L: 8, 1728L: 8, 1745L: 8, 1779L: 8, 1904L: 8, 1912L: 8, 1990L: 8, 1998L: 8
   },
+  "TOYOTA PRIUS 2017": {
+    36L: 8, 37L: 8, 166L: 8, 170L: 8, 180L: 8, 295L: 8, 296L: 8, 426L: 6, 452L: 8, 466L: 8, 467L: 8, 550L: 8, 552L: 4, 560L: 7, 562L: 6, 581L: 5, 608L: 8, 610L: 8, 614L: 8, 643L: 7, 658L: 8, 713L: 8, 740L: 5, 742L: 8, 743L: 8, 800L: 8, 810L: 2, 814L: 8, 829L: 2, 830L: 7, 835L: 8, 836L: 8, 863L: 8, 869L: 7, 870L: 7, 871L: 2, 898L: 8, 900L: 6, 902L: 6, 905L: 8, 918L: 8, 921L: 8, 933L: 8, 944L: 8, 945L: 8, 950L: 8, 951L: 8, 953L: 8, 955L: 8, 956L: 8, 971L: 7, 975L: 5, 993L: 8, 998L: 5, 999L: 7, 1000L: 8, 1001L: 8, 1014L: 8, 1017L: 8, 1020L: 8, 1041L: 8, 1042L: 8, 1044L: 8, 1056L: 8, 1057L: 8, 1059L: 1, 1071L: 8, 1077L: 8, 1082L: 8, 1083L: 8, 1084L: 8, 1085L: 8, 1086L: 8, 1114L: 8, 1132L: 8, 1161L: 8, 1162L: 8, 1163L: 8, 1175L: 8, 1227L: 8, 1228L: 8, 1235L: 8, 1237L: 8, 1279L: 8, 1552L: 8, 1553L: 8, 1556L: 8, 1557L: 8, 1568L: 8, 1570L: 8, 1571L: 8, 1572L: 8, 1595L: 8, 1777L: 8, 1779L: 8, 1904L: 8, 1912L: 8, 1990L: 8, 1998L: 8
+  },
 }
 
 # support additional internal only fingerprints

common/params.py

@@ -56,6 +56,7 @@ keys = {
 # read:    ui, controls
   "IsMetric": TxType.PERSISTANT,
   "IsRearViewMirror": TxType.PERSISTANT,
+  "IsFcwEnabled": TxType.PERSISTANT,
 # written: visiond
 # read:    visiond, controlsd
   "CalibrationParams": TxType.PERSISTANT,

opendbc

@@ -1 +1 @@
-Subproject commit 835a9739d6721e351e1814439b55b6c4212f7b85
+Subproject commit c8eeedce1717c6e05acd77f8b893908667baea21

panda

@@ -1 +1 @@
-Subproject commit 849f68879d1ceacbf1f9d4174e16e1cd14527383
+Subproject commit 3cab37297566962fd6e48a674db3e1f6de8fa4da

selfdrive/car/__init__.py

@@ -85,4 +85,4 @@ def get_car(logcan, sendcan=None, timeout=None):
   interface_cls = interfaces[candidate]
   params = interface_cls.get_params(candidate, fingerprints)
 
-  return interface_cls(params, logcan, sendcan), params
+  return interface_cls(params, sendcan), params

selfdrive/car/honda/carcontroller.py

@@ -18,7 +18,7 @@ CAMERA_MSGS = [0xe4, 0x194]
 
 def actuator_hystereses(brake, braking, brake_steady, v_ego, civic):
   # hyst params... TODO: move these to VehicleParams
-  brake_hyst_on = 0.055 if civic else 0.1    # to activate brakes exceed this value
+  brake_hyst_on = 0.02     # to activate brakes exceed this value
   brake_hyst_off = 0.005                     # to deactivate brakes below this value
   brake_hyst_gap = 0.01                      # don't change brake command for small ocilalitons within this value
 
@@ -36,14 +36,8 @@ def actuator_hystereses(brake, braking, brake_steady, v_ego, civic):
     brake_steady = brake + brake_hyst_gap
   brake = brake_steady
 
-  if not civic:
-    brake_on_offset_v  = [.25, .15]   # min brake command on brake activation. below this no decel is perceived
-    brake_on_offset_bp = [15., 30.]     # offset changes VS speed to not have too abrupt decels at high speeds
-    # offset the brake command for threshold in the brake system. no brake torque perceived below it
-    brake_on_offset = interp(v_ego, brake_on_offset_bp, brake_on_offset_v)
-    brake_offset = brake_on_offset - brake_hyst_on
-    if brake > 0.0:
-      brake += brake_offset
+  if not civic and brake > 0.0:
+    brake += 0.15
 
   return brake, braking, brake_steady
 
@@ -69,6 +63,7 @@ HUDData = namedtuple("HUDData",
                      ["pcm_accel", "v_cruise", "X2", "car", "X4", "X5",
                       "lanes", "beep", "X8", "chime", "acc_alert"])
 
+
 class CarController(object):
   def __init__(self, enable_camera=True):
     self.braking = False
@@ -95,8 +90,7 @@ class CarController(object):
       pcm_cancel_cmd = True
 
     # *** rate limit after the enable check ***
-    brake = rate_limit(brake, self.brake_last, -2., 1./100)
-    self.brake_last = brake
+    self.brake_last = rate_limit(brake, self.brake_last, -2., 1./100)
 
     # vehicle hud display, wait for one update from 10Hz 0x304 msg
     #TODO: use enum!!
@@ -142,17 +136,9 @@ class CarController(object):
 
     # steer torque is converted back to CAN reference (positive when steering right)
     apply_gas = int(clip(actuators.gas * GAS_MAX, 0, GAS_MAX - 1))
-    apply_brake = int(clip(brake * BRAKE_MAX, 0, BRAKE_MAX - 1))
+    apply_brake = int(clip(self.brake_last * BRAKE_MAX, 0, BRAKE_MAX - 1))
     apply_steer = int(clip(-actuators.steer * STEER_MAX, -STEER_MAX, STEER_MAX))
 
-    # no gas if you are hitting the brake or the user is
-    if apply_gas > 0 and (apply_brake != 0 or CS.brake_pressed):
-      apply_gas = 0
-
-    # no computer brake if the gas is being pressed
-    if CS.car_gas > 0 and apply_brake != 0:
-      apply_brake = 0
-
     # any other cp.vl[0x18F]['STEER_STATUS'] is common and can happen during user override. sending 0 torque to avoid EPS sending error 5
     if CS.steer_not_allowed:
       apply_steer = 0

selfdrive/car/honda/carstate.py

@@ -2,7 +2,6 @@ import os
 import time
 from cereal import car
 from common.numpy_fast import interp
-from common.realtime import sec_since_boot
 import selfdrive.messaging as messaging
 from selfdrive.can.parser import CANParser
 from selfdrive.config import Conversions as CV
@@ -275,7 +274,7 @@ def get_can_parser(CP):
   return CANParser(os.path.splitext(dbc_f)[0], signals, checks, 0)
 
 class CarState(object):
-  def __init__(self, CP, logcan):
+  def __init__(self, CP):
     self.acura = False
     self.civic = False
     self.accord = False
@@ -294,9 +293,6 @@ class CarState(object):
     self.brake_only = CP.enableCruise
     self.CP = CP
 
-    # initialize can parser
-    self.cp = get_can_parser(CP)
-
     self.user_gas, self.user_gas_pressed = 0., 0
     self.brake_switch_prev = 0
     self.brake_switch_ts = 0
@@ -315,14 +311,13 @@ class CarState(object):
     self.v_ego_C = np.matrix([1.0, 0.0])
     self.v_ego_Q = np.matrix([[10.0, 0.0], [0.0, 100.0]])
     self.v_ego_R = 1e3
+    self.v_ego = 0.0
     # import control
     # (x, l, K) = control.dare(np.transpose(A), np.transpose(C), Q, R)
     # self.v_ego_K = np.transpose(K)
     self.v_ego_K = np.matrix([[0.12287673], [0.29666309]])
 
-  def update(self, can_pub_main=None):
-    cp = self.cp
-    cp.update(int(sec_since_boot() * 1e9), False)
+  def update(self, cp):
 
     # copy can_valid
     self.can_valid = cp.can_valid
@@ -368,6 +363,9 @@ class CarState(object):
     # blend in transmission speed at low speed, since it has more low speed accuracy
     self.v_weight = interp(self.v_wheel, v_weight_bp, v_weight_v)
     speed = (1. - self.v_weight) * cp.vl[0x158]['XMISSION_SPEED'] + self.v_weight * self.v_wheel
+
+    if abs(speed - self.v_ego) > 2.0:  # Prevent large accelerations when car starts at non zero speed
+      self.v_ego_x = np.matrix([[speed], [0.0]])
     self.v_ego_x = np.dot((self.v_ego_A - np.dot(self.v_ego_K, self.v_ego_C)), self.v_ego_x) + np.dot(self.v_ego_K, speed)
 
     self.v_ego_raw = speed
@@ -449,13 +447,15 @@ class CarState(object):
     else:
       self.car_gas = cp.vl[0x130]['CAR_GAS']
       self.steer_override = abs(cp.vl[0x18F]['STEER_TORQUE_SENSOR']) > 1200
+    self.steer_torque_driver = cp.vl[0x18F]['STEER_TORQUE_SENSOR']
 
     # brake switch has shown some single time step noise, so only considered when
     # switch is on for at least 2 consecutive CAN samples
+    self.brake_switch = cp.vl[0x17C]['BRAKE_SWITCH']
     self.brake_pressed = cp.vl[0x17C]['BRAKE_PRESSED'] or \
-                         (cp.vl[0x17C]['BRAKE_SWITCH'] and self.brake_switch_prev and \
+                         (self.brake_switch and self.brake_switch_prev and \
                          cp.ts[0x17C]['BRAKE_SWITCH'] != self.brake_switch_ts)
-    self.brake_switch_prev = cp.vl[0x17C]['BRAKE_SWITCH']
+    self.brake_switch_prev = self.brake_switch
     self.brake_switch_ts = cp.ts[0x17C]['BRAKE_SWITCH']
 
     self.user_brake = cp.vl[0x1A4]['USER_BRAKE']
@@ -472,7 +472,6 @@ if __name__ == '__main__':
   import time
   from selfdrive.services import service_list
   context = zmq.Context()
-  logcan = messaging.sub_sock(context, service_list['can'].port)
 
   class CarParams(object):
     def __init__(self):
@@ -480,7 +479,7 @@ if __name__ == '__main__':
       self.enableGas = 0
       self.enableCruise = 0
   CP = CarParams()
-  CS = CarState(CP, logcan)
+  CS = CarState(CP)
 
   while 1:
     CS.update()

selfdrive/car/honda/interface.py

@@ -2,23 +2,34 @@
 import os
 import time
 import numpy as np
-from common.numpy_fast import clip
+from common.numpy_fast import clip, interp
 from common.realtime import sec_since_boot
 from selfdrive.config import Conversions as CV
 from selfdrive.controls.lib.drive_helpers import create_event, EventTypes as ET, get_events
 from cereal import car
 from selfdrive.services import service_list
 import selfdrive.messaging as messaging
-from selfdrive.car.honda.carstate import CarState
+from selfdrive.car.honda.carstate import CarState, get_can_parser
 from selfdrive.car.honda.carcontroller import CAMERA_MSGS
 from selfdrive.car.honda.values import CruiseButtons, CM, BP, AH
+from selfdrive.controls.lib.planner import A_ACC_MAX
 
 try:
   from .carcontroller import CarController
 except ImportError:
   CarController = None
 
-def get_compute_gb():
+
+def compute_gb_honda(accel, speed):
+  creep_brake = 0.0
+  creep_speed = 2.3
+  creep_brake_value = 0.15
+  if speed < creep_speed:
+    creep_brake = (creep_speed - speed) / creep_speed * creep_brake_value
+  return float(accel) / 4.8 - creep_brake
+
+
+def get_compute_gb_acura():
   # generate a function that takes in [desired_accel, current_speed] -> [-1.0, 1.0]
   # where -1.0 is max brake and 1.0 is max gas
   # see debug/dump_accel_from_fiber.py to see how those parameters were generated
@@ -45,8 +56,8 @@ def get_compute_gb():
   def leakyrelu(x, alpha):
     return np.maximum(x, alpha * x)
 
-  def _compute_gb(accel, speed):
-    #linearly extrap below v1 using v1 and v2 data
+  def _compute_gb_acura(accel, speed):
+    # linearly extrap below v1 using v1 and v2 data
     v1 = 5.
     v2 = 10.
     dat = np.array([accel, speed])
@@ -60,12 +71,11 @@ def get_compute_gb():
       m4 = (speed - v1) * (m4v2 - m4v1) / (v2 - v1) + m4v1
     return float(m4)
 
-  return _compute_gb
+  return _compute_gb_acura
 
 
 class CarInterface(object):
-  def __init__(self, CP, logcan, sendcan=None):
-    self.logcan = logcan
+  def __init__(self, CP, sendcan=None):
     self.CP = CP
 
     self.frame = 0
@@ -75,8 +85,10 @@ class CarInterface(object):
     self.brake_pressed_prev = False
     self.can_invalid_count = 0
 
+    self.cp = get_can_parser(CP)
+
     # *** init the major players ***
-    self.CS = CarState(CP, self.logcan)
+    self.CS = CarState(CP)
 
     # sending if read only is False
     if sendcan is not None:
@@ -87,6 +99,25 @@ class CarInterface(object):
       # self.accord_msg = []
       raise NotImplementedError
 
+    if not self.CS.civic:
+      self.compute_gb = get_compute_gb_acura()
+    else:
+      self.compute_gb = compute_gb_honda
+
+  @staticmethod
+  def calc_accel_override(a_ego, a_target, v_ego, v_target):
+    eA = a_ego - a_target
+    valuesA = [1.0, 0.1]
+    bpA = [0.0, 0.5]
+
+    eV = v_ego - v_target
+    valuesV = [1.0, 0.1]
+    bpV = [0.0, 0.5]
+
+    # accelOverride is more or less the max throttle allowed to pcm: usually set to a constant
+    # unless aTargetMax is very high and then we scale with it; this help in quicker restart
+    return float(max(0.714, a_target / A_ACC_MAX)) * min(interp(eA, bpA, valuesA), interp(eV, bpV, valuesV))
+
   @staticmethod
   def get_params(candidate, fingerprint):
 
@@ -130,6 +161,11 @@ class CarInterface(object):
       # Civic at comma has modified steering FW, so different tuning for the Neo in that car
       is_fw_modified = os.getenv("DONGLE_ID") in ['b0f5a01cf604185c']
       ret.steerKp, ret.steerKi = [0.4, 0.12] if is_fw_modified else [0.8, 0.24]
+
+      ret.longitudinalKpBP = [0., 5., 35.]
+      ret.longitudinalKpV = [3.6, 2.4, 1.5]
+      ret.longitudinalKiBP = [0., 35.]
+      ret.longitudinalKiV = [0.54, 0.36]
     elif candidate == "ACURA ILX 2016 ACURAWATCH PLUS":
       stop_and_go = False
       ret.m = 3095./2.205 + std_cargo
@@ -139,6 +175,11 @@ class CarInterface(object):
       # Acura at comma has modified steering FW, so different tuning for the Neo in that car
       is_fw_modified = os.getenv("DONGLE_ID") in ['cb38263377b873ee']
       ret.steerKp, ret.steerKi = [0.4, 0.12] if is_fw_modified else [0.8, 0.24]
+
+      ret.longitudinalKpBP = [0., 5., 35.]
+      ret.longitudinalKpV = [1.2, 0.8, 0.5]
+      ret.longitudinalKiBP = [0., 35.]
+      ret.longitudinalKiV = [0.18, 0.12]
     elif candidate == "HONDA ACCORD 2016 TOURING":
       stop_and_go = False
       ret.m = 3580./2.205 + std_cargo
@@ -146,6 +187,11 @@ class CarInterface(object):
       ret.aF = ret.l * 0.38
       ret.sR = 15.3
       ret.steerKp, ret.steerKi = 0.8, 0.24
+
+      ret.longitudinalKpBP = [0., 5., 35.]
+      ret.longitudinalKpV = [1.2, 0.8, 0.5]
+      ret.longitudinalKiBP = [0., 35.]
+      ret.longitudinalKiV = [0.18, 0.12]
     elif candidate == "HONDA CR-V 2016 TOURING":
       stop_and_go = False
       ret.m = 3572./2.205 + std_cargo
@@ -153,6 +199,11 @@ class CarInterface(object):
       ret.aF = ret.l * 0.41
       ret.sR = 15.3
       ret.steerKp, ret.steerKi = 0.8, 0.24
+
+      ret.longitudinalKpBP = [0., 5., 35.]
+      ret.longitudinalKpV = [1.2, 0.8, 0.5]
+      ret.longitudinalKiBP = [0., 35.]
+      ret.longitudinalKiV = [0.18, 0.12]
     else:
       raise ValueError("unsupported car %s" % candidate)
 
@@ -188,19 +239,20 @@ class CarInterface(object):
     ret.longPidDeadzoneBP = [0.]
     ret.longPidDeadzoneV = [0.]
 
+    ret.stoppingControl = True
     ret.steerLimitAlert = True
+    ret.startAccel = 0.5
 
     return ret
 
-  compute_gb = staticmethod(get_compute_gb())
-
   # returns a car.CarState
   def update(self, c):
     # ******************* do can recv *******************
-    can_pub_main = []
     canMonoTimes = []
 
-    self.CS.update(can_pub_main)
+    self.cp.update(int(sec_since_boot() * 1e9), False)
+
+    self.CS.update(self.cp)
 
     # create message
     ret = car.CarState.new_message()
@@ -225,6 +277,10 @@ class CarInterface(object):
     # brake pedal
     ret.brake = self.CS.user_brake
     ret.brakePressed = self.CS.brake_pressed != 0
+    # FIXME: read sendcan for brakelights
+    brakelights_threshold = 0.02 if self.CS.civic else 0.1
+    ret.brakeLights = bool(self.CS.brake_switch or
+                           c.actuators.brake > brakelights_threshold)
 
     # steering wheel
     ret.steeringAngle = self.CS.angle_steers
@@ -233,7 +289,7 @@ class CarInterface(object):
     # gear shifter lever
     ret.gearShifter = self.CS.gear_shifter
 
-    ret.steeringTorque = self.CS.cp.vl[0x18F]['STEER_TORQUE_SENSOR']
+    ret.steeringTorque = self.CS.steer_torque_driver
     ret.steeringPressed = self.CS.steer_override
 
     # cruise state
@@ -241,9 +297,12 @@ class CarInterface(object):
     ret.cruiseState.speed = self.CS.v_cruise_pcm * CV.KPH_TO_MS
     ret.cruiseState.available = bool(self.CS.main_on)
     ret.cruiseState.speedOffset = self.CS.cruise_speed_offset
+    ret.cruiseState.standstill = False
 
     # TODO: button presses
     buttonEvents = []
+    ret.leftBlinker = bool(self.CS.left_blinker_on)
+    ret.rightBlinker = bool(self.CS.right_blinker_on)
 
     if self.CS.left_blinker_on != self.CS.prev_left_blinker_on:
       be = car.CarState.ButtonEvent.new_message()
@@ -332,16 +391,16 @@ class CarInterface(object):
        (ret.brakePressed and (not self.brake_pressed_prev or ret.vEgo > 0.001)):
       events.append(create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE]))
 
-    #if (ret.brakePressed and ret.vEgo < 0.001) or ret.gasPressed:
     if ret.gasPressed:
       events.append(create_event('pedalPressed', [ET.PRE_ENABLE]))
 
     # it can happen that car cruise disables while comma system is enabled: need to
     # keep braking if needed or if the speed is very low
     # TODO: for the Acura, cancellation below 25mph is normal. Issue a non loud alert
-    if self.CP.enableCruise and not ret.cruiseState.enabled and \
-       (c.actuators.brake <= 0. or ret.vEgo < 0.3):
+    if self.CP.enableCruise and not ret.cruiseState.enabled and c.actuators.brake <= 0.:
       events.append(create_event("cruiseDisabled", [ET.IMMEDIATE_DISABLE]))
+    if not self.CS.civic and ret.vEgo < 0.001:
+      events.append(create_event('manualRestart', [ET.WARNING]))
 
     cur_time = sec_since_boot()
     enable_pressed = False
@@ -380,14 +439,11 @@ class CarInterface(object):
     self.brake_pressed_prev = ret.brakePressed
 
     # cast to reader so it can't be modified
-    #print ret
     return ret.as_reader()
 
   # pass in a car.CarControl
   # to be called @ 100hz
   def apply(self, c):
-    #print c
-
     if c.hudControl.speedVisible:
       hud_v_cruise = c.hudControl.setSpeed * CV.MS_TO_KPH
     else:

selfdrive/car/toyota/carcontroller.py

@@ -4,7 +4,9 @@ from selfdrive.boardd.boardd import can_list_to_can_capnp
 from selfdrive.controls.lib.drive_helpers import rate_limit
 from selfdrive.car.toyota.toyotacan import make_can_msg, create_video_target,\
                                            create_steer_command, create_ui_command, \
-                                           create_ipas_steer_command, create_accel_command
+                                           create_ipas_steer_command, create_accel_command, \
+                                           create_fcw_command
+from selfdrive.car.toyota.values import CAR, ECU
 
 
 ACCEL_HYST_GAP = 0.02  # don't change accel command for small oscilalitons within this value
@@ -14,22 +16,12 @@ ACCEL_SCALE = max(ACCEL_MAX, -ACCEL_MIN)
 
 STEER_MAX = 1500
 STEER_DELTA_UP = 10        # 1.5s time to peak torque
-STEER_DELTA_DOWN = 45      # lower than 50 otherwise the Rav4 faults (Prius seems ok though)
+STEER_DELTA_DOWN = 25      # always lower than 45 otherwise the Rav4 faults (Prius seems ok with 50)
 STEER_ERROR_MAX = 500      # max delta between torque cmd and torque motor
 
 STEER_IPAS_MAX = 340
 STEER_IPAS_DELTA_MAX = 3
 
-class CAR:
-  PRIUS = "TOYOTA PRIUS 2017"
-  RAV4 = "TOYOTA RAV4 2017"
-
-class ECU:
-  CAM = 0 # camera
-  DSU = 1 # driving support unit
-  APGS = 2 # advanced parking guidance system
-
-
 TARGET_IDS = [0x340, 0x341, 0x342, 0x343, 0x344, 0x345,
               0x363, 0x364, 0x365, 0x370, 0x371, 0x372,
               0x373, 0x374, 0x375, 0x380, 0x381, 0x382,
@@ -75,7 +67,6 @@ STATIC_MSGS = {0x141: (ECU.DSU, (CAR.PRIUS, CAR.RAV4), 1,   2, '\x00\x00\x00\x46
                0x43B: (ECU.APGS, (CAR.PRIUS, CAR.RAV4), 0, 100, '\x00\x00\x00\x00\x00\x00\x00\x00'),
                0x497: (ECU.APGS, (CAR.PRIUS, CAR.RAV4), 0, 100, '\x00\x00\x00\x00\x00\x00\x00\x00'),
                0x4CC: (ECU.APGS, (CAR.PRIUS, CAR.RAV4), 0, 100, '\x0D\x00\x00\x00\x00\x00\x00\x00'),
-               0x411: (ECU.DSU, (CAR.PRIUS, CAR.RAV4), 0, 100, '\x00\x20\x00\x00\x10\x00\x80\x00'),
                0x4CB: (ECU.DSU, (CAR.PRIUS, CAR.RAV4), 0, 100, '\x0c\x00\x00\x00\x00\x00\x00\x00'),
                0x470: (ECU.DSU, (CAR.PRIUS,), 1, 100, '\x00\x00\x02\x7a'),
               }
@@ -107,18 +98,23 @@ def accel_hysteresis(accel, accel_steady, enabled):
 
 def process_hud_alert(hud_alert, audible_alert):
   # initialize to no alert
-  hud1 = 0
-  hud2 = 0
-  if hud_alert in ['steerRequired', 'fcw']:
-    if audible_alert == 'chimeRepeated':
-      hud1 = 3
-    else:
-      hud1 = 1
-  if audible_alert in ['beepSingle', 'chimeSingle', 'chimeDouble']:
+  steer = 0
+  fcw = 0
+  sound1 = 0
+  sound2 = 0
+
+  if hud_alert == 'fcw':
+    fcw = 1
+  elif hud_alert == 'steerRequired':
+    steer = 1
+
+  if audible_alert == 'chimeRepeated':
+    sound1 = 1
+  elif audible_alert in ['beepSingle', 'chimeSingle', 'chimeDouble']:
     # TODO: find a way to send single chimes
-    hud2 = 1
+    sound2 = 1
 
-  return hud1, hud2
+  return steer, fcw, sound1, sound2
 
 
 class CarController(object):
@@ -130,6 +126,8 @@ class CarController(object):
     self.accel_steady = 0.
     self.car_fingerprint = car_fingerprint
     self.alert_active = False
+    self.last_standstill = False
+    self.standstill_req = False
 
     self.fake_ecus = set()
     if enable_camera: self.fake_ecus.add(ECU.CAM)
@@ -140,7 +138,7 @@ class CarController(object):
              pcm_cancel_cmd, hud_alert, audible_alert):
 
     # *** compute control surfaces ***
-    tt = sec_since_boot()
+    ts = sec_since_boot()
 
     # steer torque is converted back to CAN reference (positive when steering right)
     apply_accel = actuators.gas - actuators.brake
@@ -170,8 +168,16 @@ class CarController(object):
     if not enabled or CS.steer_error:
       apply_steer = 0
 
+    # on entering standstill, send standstill request
+    if CS.standstill and not self.last_standstill:
+      self.standstill_req = True
+    if CS.pcm_acc_status != 8:
+      # pcm entered standstill or it's disabled
+      self.standstill_req = False
+
     self.last_steer = apply_steer
     self.last_accel = apply_accel
+    self.last_standstill = CS.standstill
 
     can_sends = []
 
@@ -190,9 +196,9 @@ class CarController(object):
     # accel cmd comes from DSU, but we can spam can to cancel the system even if we are using lat only control
     if (frame % 3 == 0 and ECU.DSU in self.fake_ecus) or (pcm_cancel_cmd and ECU.CAM in self.fake_ecus):
       if ECU.DSU in self.fake_ecus:
-        can_sends.append(create_accel_command(apply_accel, pcm_cancel_cmd))
+        can_sends.append(create_accel_command(apply_accel, pcm_cancel_cmd, self.standstill_req))
       else:
-        can_sends.append(create_accel_command(0, pcm_cancel_cmd))
+        can_sends.append(create_accel_command(0, pcm_cancel_cmd, False))
 
     if frame % 10 == 0 and ECU.CAM in self.fake_ecus:
       for addr in TARGET_IDS:
@@ -201,24 +207,25 @@ class CarController(object):
     # ui mesg is at 100Hz but we send asap if:
     # - there is something to display
     # - there is something to stop displaying
-    hud1, hud2 = process_hud_alert(hud_alert, audible_alert)
-    if ((hud1 or hud2) and not self.alert_active) or \
-       (not (hud1 or hud2) and self.alert_active):
+    alert_out = process_hud_alert(hud_alert, audible_alert)
+    steer, fcw, sound1, sound2 = alert_out
+
+    if (any(alert_out) and not self.alert_active) or \
+       (not any(alert_out) and self.alert_active):
       send_ui = True
       self.alert_active = not self.alert_active
     else:
       send_ui = False
 
     if (frame % 100 == 0 or send_ui) and ECU.CAM in self.fake_ecus:
-      can_sends.append(create_ui_command(hud1, hud2))
+      can_sends.append(create_ui_command(steer, sound1, sound2))
+      can_sends.append(create_fcw_command(fcw))
 
     #*** static msgs ***
 
     for addr, (ecu, cars, bus, fr_step, vl) in STATIC_MSGS.iteritems():
       if frame % fr_step == 0 and ecu in self.fake_ecus and self.car_fingerprint in cars:
-        # send msg!
         # special cases
-
         if fr_step == 5 and ecu == ECU.CAM and bus == 1:
           cnt = (((frame / 5) % 7) + 1) << 5
           vl = chr(cnt) + vl

selfdrive/car/toyota/carstate.py

@@ -1,8 +1,7 @@
 import os
 import time
-from common.realtime import sec_since_boot
 import selfdrive.messaging as messaging
-from selfdrive.car.toyota.carcontroller import CAR
+from selfdrive.car.toyota.values import CAR
 from selfdrive.can.parser import CANParser
 from selfdrive.config import Conversions as CV
 import numpy as np
@@ -85,6 +84,7 @@ def get_can_parser(CP):
     ("STEER_TORQUE_EPS", 608, 0),
     ("TURN_SIGNALS", 1556, 3),   # 3 is no blinkers
     ("LKA_STATE", 610, 0),
+    ("BRAKE_LIGHTS_ACC", 951, 0),
   ]
 
   checks += [
@@ -100,14 +100,13 @@ def get_can_parser(CP):
 
 
 class CarState(object):
-  def __init__(self, CP, logcan):
+  def __init__(self, CP):
 
     self.CP = CP
     self.left_blinker_on = 0
     self.right_blinker_on = 0
 
     # initialize can parser
-    self.cp = get_can_parser(CP)
     self.car_fingerprint = CP.carFingerprint
 
     # vEgo kalman filter
@@ -117,14 +116,13 @@ class CarState(object):
     self.v_ego_C = np.matrix([1.0, 0.0])
     self.v_ego_Q = np.matrix([[10.0, 0.0], [0.0, 100.0]])
     self.v_ego_R = 1e3
+    self.v_ego = 0.0
     # import control
     # (x, l, K) = control.dare(np.transpose(A), np.transpose(C), Q, R)
     # self.v_ego_K = np.transpose(K)
     self.v_ego_K = np.matrix([[0.12287673], [0.29666309]])
 
-  def update(self):
-    cp = self.cp
-    cp.update(int(sec_since_boot() * 1e9), False)
+  def update(self, cp):
 
     # copy can_valid
     self.can_valid = cp.can_valid
@@ -160,6 +158,8 @@ class CarState(object):
       cp.vl[170]['WHEEL_SPEED_RL'] + cp.vl[170]['WHEEL_SPEED_RR']) / 4. * CV.KPH_TO_MS
 
     # Kalman filter
+    if abs(self.v_wheel - self.v_ego) > 2.0:  # Prevent large accelerations when car starts at non zero speed
+      self.v_ego_x = np.matrix([[self.v_wheel], [0.0]])
     self.v_ego_x = np.dot((self.v_ego_A - np.dot(self.v_ego_K, self.v_ego_C)), self.v_ego_x) + np.dot(self.v_ego_K, self.v_wheel)
     self.v_ego_raw = self.v_wheel
     self.v_ego = float(self.v_ego_x[0])
@@ -185,3 +185,4 @@ class CarState(object):
     self.car_gas = self.pedal_gas
     self.gas_pressed = not cp.vl[466]['GAS_RELEASED']
     self.low_speed_lockout = cp.vl[467]['LOW_SPEED_LOCKOUT'] == 2
+    self.brake_lights = bool(cp.vl[951]['BRAKE_LIGHTS_ACC'] or self.brake_pressed)

selfdrive/car/toyota/interface.py

@@ -1,19 +1,19 @@
 #!/usr/bin/env python
 import os
 import time
+from common.realtime import sec_since_boot
 import common.numpy_fast as np
 from selfdrive.config import Conversions as CV
-from selfdrive.car.toyota.carstate import CarState, CAR
-from selfdrive.car.toyota.carcontroller import CarController, ECU, check_ecu_msgs
+from selfdrive.car.toyota.carstate import CarState, get_can_parser
+from selfdrive.car.toyota.values import CAR, ECU
+from selfdrive.car.toyota.carcontroller import CarController, check_ecu_msgs
 from cereal import car
 from selfdrive.services import service_list
 import selfdrive.messaging as messaging
 from selfdrive.controls.lib.drive_helpers import EventTypes as ET, create_event
 
-
 class CarInterface(object):
-  def __init__(self, CP, logcan, sendcan=None):
-    self.logcan = logcan
+  def __init__(self, CP, sendcan=None):
     self.CP = CP
 
     self.frame = 0
@@ -23,13 +23,23 @@ class CarInterface(object):
     self.cruise_enabled_prev = False
 
     # *** init the major players ***
-    self.CS = CarState(CP, self.logcan)
+    self.CS = CarState(CP)
+
+    self.cp = get_can_parser(CP)
 
     # sending if read only is False
     if sendcan is not None:
       self.sendcan = sendcan
       self.CC = CarController(CP.carFingerprint, CP.enableCamera, CP.enableDsu, CP.enableApgs)
 
+  @staticmethod
+  def compute_gb(accel, speed):
+    return float(accel) / 3.0
+
+  @staticmethod
+  def calc_accel_override(a_ego, a_target, v_ego, v_target):
+    return 1.0
+
   @staticmethod
   def get_params(candidate, fingerprint):
 
@@ -108,13 +118,15 @@ class CarInterface(object):
     ret.enableGas = True
 
     ret.steerLimitAlert = False
+    ret.stoppingControl = False
+    ret.startAccel = 0.0
 
-    return ret
+    ret.longitudinalKpBP = [0., 5., 35.]
+    ret.longitudinalKpV = [3.6, 2.4, 1.5]
+    ret.longitudinalKiBP = [0., 35.]
+    ret.longitudinalKiV = [0.54, 0.36]
 
-  @staticmethod
-  def compute_gb(accel, speed):
-    # toyota interface is already in accelration cmd, so conversion to gas-brake it's a pass-through.
-    return accel
+    return ret
 
   # returns a car.CarState
   def update(self, c):
@@ -122,7 +134,9 @@ class CarInterface(object):
     can_pub_main = []
     canMonoTimes = []
 
-    self.CS.update()
+    self.cp.update(int(sec_since_boot() * 1e9), False)
+
+    self.CS.update(self.cp)
 
     # create message
     ret = car.CarState.new_message()
@@ -147,6 +161,7 @@ class CarInterface(object):
     # brake pedal
     ret.brake = self.CS.user_brake
     ret.brakePressed = self.CS.brake_pressed != 0
+    ret.brakeLights = self.CS.brake_lights
 
     # steering wheel
     ret.steeringAngle = self.CS.angle_steers
@@ -160,6 +175,7 @@ class CarInterface(object):
     ret.cruiseState.speed = self.CS.v_cruise_pcm * CV.KPH_TO_MS
     ret.cruiseState.available = bool(self.CS.main_on)
     ret.cruiseState.speedOffset = 0.
+    ret.cruiseState.standstill = self.CS.pcm_acc_status == 7
 
     # TODO: button presses
     buttonEvents = []
@@ -177,6 +193,8 @@ class CarInterface(object):
       buttonEvents.append(be)
 
     ret.buttonEvents = buttonEvents
+    ret.leftBlinker = bool(self.CS.left_blinker_on)
+    ret.rightBlinker = bool(self.CS.right_blinker_on)
 
     # events
     events = []

selfdrive/car/toyota/toyotacan.py

@@ -68,16 +68,24 @@ def create_steer_command(steer, raw_cnt):
   return make_can_msg(0x2e4, msg, 0, True)
 
 
-def create_accel_command(accel, pcm_cancel):
+def create_accel_command(accel, pcm_cancel, standstill_req):
   # TODO: find the exact canceling bit
-  state = 0xc0 + pcm_cancel # this allows automatic restart from hold without driver cmd
+  state = 0x40 if standstill_req else 0xC0
+  state += pcm_cancel # this allows automatic restart from hold without driver cmd
 
   msg = struct.pack("!hBBBBB", accel, 0x63, state, 0x00, 0x00, 0x00)
 
   return make_can_msg(0x343, msg, 0, True)
 
+def create_fcw_command(fcw):
 
-def create_ui_command(hud1, hud2):
-  msg = struct.pack('!BBBBBBBB', 0x54, 0x04 + hud1 + (hud2 << 4), 0x0C, 
-                                 0x00, 0x00, 0x2C, 0x38, 0x02) 
+  msg = struct.pack("!BBBBBBBB", fcw<<4, 0x20, 0x00, 0x00, 0x10, 0x00, 0x80, 0x00)
+
+  return make_can_msg(0x411, msg, 0, False)
+
+
+def create_ui_command(steer, sound1, sound2):
+
+  msg = struct.pack("!BBBBBBBB", 0x54, 0x04 + steer + (sound2<<4), 0x0C, 0x00, 
+                                 sound1, 0x2C, 0x38, 0x02)
   return make_can_msg(0x412, msg, 0, False)

selfdrive/car/toyota/values.py

@@ -0,0 +1,8 @@
+class CAR: 
+  PRIUS = "TOYOTA PRIUS 2017" 
+  RAV4 = "TOYOTA RAV4 2017" 
+ 
+class ECU: 
+  CAM = 0 # camera 
+  DSU = 1 # driving support unit 
+  APGS = 2 # advanced parking guidance system 

selfdrive/common/version.h

@@ -1 +1 @@
-#define COMMA_VERSION "0.3.8.2-openpilot"
+#define COMMA_VERSION "0.3.9-openpilot"

selfdrive/config.py

@@ -21,12 +21,14 @@ class Conversions:
 
 
 # Car button codes
+# TODO: this is Honda specific, move to honda/interface.py
 class CruiseButtons:
   RES_ACCEL   = 4
   DECEL_SET   = 3
   CANCEL      = 2
   MAIN        = 1
 
+RADAR_TO_CENTER = 2.7   # RADAR is ~ 2.7m ahead from center of car
 
 # Image params for color cam on acura, calibrated on pre las vegas drive (2016-05-21)
 class ImageParams:

selfdrive/controls/controlsd.py

@@ -1,15 +1,13 @@
 #!/usr/bin/env python
-import os
 import json
 from copy import copy
 import zmq
-from cereal import car, log
+from cereal import car
 from common.numpy_fast import clip
 from common.realtime import sec_since_boot, set_realtime_priority, Ratekeeper
 from common.profiler import Profiler
 from common.params import Params
 import selfdrive.messaging as messaging
-from selfdrive.swaglog import cloudlog
 from selfdrive.config import Conversions as CV
 from selfdrive.services import service_list
 from selfdrive.car import get_car
@@ -22,7 +20,7 @@ from selfdrive.controls.lib.longcontrol import LongControl, STARTING_TARGET_SPEE
 from selfdrive.controls.lib.latcontrol import LatControl
 from selfdrive.controls.lib.alertmanager import AlertManager
 from selfdrive.controls.lib.vehicle_model import VehicleModel
-from selfdrive.controls.lib.adaptivecruise import A_ACC_MAX
+
 
 V_CRUISE_MAX = 144
 V_CRUISE_MIN = 8
@@ -31,7 +29,6 @@ V_CRUISE_ENABLE_MIN = 40
 
 AWARENESS_TIME = 360.      # 6 minutes limit without user touching steering wheels
 AWARENESS_PRE_TIME = 20.   # a first alert is issued 20s before start decelerating the car
-AWARENESS_DECEL = -0.2     # car smoothly decel at .2m/s^2 when user is distracted
 
 
 class Calibration:
@@ -103,9 +100,9 @@ def data_sample(CI, CC, thermal, health, cal, cal_status, overtemp, free_space):
   return CS, events, cal_status, overtemp, free_space
 
 
-def calc_plan(CS, events, PL, LoC):
+def calc_plan(CS, events, PL, LoC, v_cruise_kph, awareness_status):
    # plan runs always, independently of the state
-   plan_packet = PL.update(CS, LoC)
+   plan_packet = PL.update(CS, LoC, v_cruise_kph, awareness_status < -0.)
    plan = plan_packet.plan
    plan_ts = plan_packet.logMonoTime
 
@@ -114,7 +111,7 @@ def calc_plan(CS, events, PL, LoC):
 
    # disable if lead isn't close when system is active and brake is pressed to avoid
    # unexpected vehicle accelerations
-   if CS.brakePressed and plan.vTarget >= STARTING_TARGET_SPEED:
+   if CS.brakePressed and plan.vTargetFuture >= STARTING_TARGET_SPEED:
      events.append(create_event('noTarget', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
 
    return plan, plan_ts
@@ -265,24 +262,18 @@ def state_control(plan, CS, CP, state, events, v_cruise_kph, AM, rk, awareness_s
     for e in get_events(events, [ET.WARNING]):
       AM.add(e, enabled)
 
-  # if user is not responsive to awareness alerts, then start a smooth deceleration
-  if awareness_status < -0.:
-    plan.aTargetMax = min(plan.aTargetMax, AWARENESS_DECEL)
-    plan.aTargetMin = min(plan.aTargetMin, plan.aTargetMax)
-
   # *** angle offset learning ***
 
   if rk.frame % 5 == 2 and plan.lateralValid:
     # *** run this at 20hz again ***
     angle_offset = learn_angle_offset(active, CS.vEgo, angle_offset,
-                                      PL.PP.c_poly, PL.PP.c_prob, LaC.y_des,
+                                      PL.PP.c_poly, PL.PP.c_prob, CS.steeringAngle,
                                       CS.steeringPressed)
 
   # *** gas/brake PID loop ***
-  actuators.gas, actuators.brake = LoC.update(active, CS.vEgo, CS.brakePressed, CS.standstill,
-                                              v_cruise_kph, plan.vTarget,
-                                              [plan.aTargetMin, plan.aTargetMax],
-                                              plan.jerkFactor, CP)
+  actuators.gas, actuators.brake = LoC.update(active, CS.vEgo, CS.brakePressed, CS.standstill, CS.cruiseState.standstill,
+                                              v_cruise_kph, plan.vTarget, plan.vTargetFuture, plan.aTarget,
+                                              CP, PL.lead_1)
 
   # *** steering PID loop ***
   actuators.steer = LaC.update(active, CS.vEgo, CS.steeringAngle,
@@ -323,11 +314,7 @@ def data_send(plan, plan_ts, CS, CI, CP, state, events, actuators, v_cruise_kph,
     # brake discount removes a sharp nonlinearity
     brake_discount = (1.0 - clip(actuators.brake*3., 0.0, 1.0))
     CC.cruiseControl.speedOverride = float(max(0.0, (LoC.v_pid + CS.cruiseState.speedOffset) * brake_discount) if CP.enableCruise else 0.0)
-
-    # TODO: parametrize 0.714 in interface?
-    # accelOverride is more or less the max throttle allowed to pcm: usually set to a constant
-    # unless aTargetMax is very high and then we scale with it; this helpw in quicker restart
-    CC.cruiseControl.accelOverride = float(max(0.714, plan.aTargetMax/A_ACC_MAX))
+    CC.cruiseControl.accelOverride = CI.calc_accel_override(CS.aEgo, plan.aTarget, CS.vEgo, plan.vTarget)
 
     CC.hudControl.setSpeed = float(v_cruise_kph * CV.KPH_TO_MS)
     CC.hudControl.speedVisible = isEnabled(state)
@@ -356,6 +343,7 @@ def data_send(plan, plan_ts, CS, CI, CP, state, events, actuators, v_cruise_kph,
 
   # if controls is enabled
   dat.live100.enabled = isEnabled(state)
+  dat.live100.active = isActive(state)
 
   # car state
   dat.live100.vEgo = CS.vEgo
@@ -372,17 +360,17 @@ def data_send(plan, plan_ts, CS, CI, CP, state, events, actuators, v_cruise_kph,
   dat.live100.vCruise = float(v_cruise_kph)
   dat.live100.upAccelCmd = float(LoC.pid.p)
   dat.live100.uiAccelCmd = float(LoC.pid.i)
+  dat.live100.ufAccelCmd = float(LoC.pid.f)
 
   # lateral control state
-  dat.live100.yDes = float(LaC.y_des)
   dat.live100.angleSteersDes = float(LaC.angle_steers_des)
   dat.live100.upSteer = float(LaC.pid.p)
   dat.live100.uiSteer = float(LaC.pid.i)
+  dat.live100.ufSteer = float(LaC.pid.f)
 
   # processed radar state, should add a_pcm?
   dat.live100.vTargetLead = float(plan.vTarget)
-  dat.live100.aTargetMin = float(plan.aTargetMin)
-  dat.live100.aTargetMax = float(plan.aTargetMax)
+  dat.live100.aTarget = float(plan.aTarget)
   dat.live100.jerkFactor = float(plan.jerkFactor)
 
   # log learned angle offset
@@ -459,8 +447,10 @@ def controlsd_thread(gctx, rate=100):
   if passive:
     CP.safetyModel = car.CarParams.SafetyModels.noOutput
 
-  PL = Planner(CP)
-  LoC = LongControl(CI.compute_gb)
+  fcw_enabled = params.get("IsFcwEnabled") == "1"
+
+  PL = Planner(CP, fcw_enabled)
+  LoC = LongControl(CP, CI.compute_gb)
   VM = VehicleModel(CP)
   LaC = LatControl(VM)
   AM = AlertManager()
@@ -481,7 +471,7 @@ def controlsd_thread(gctx, rate=100):
   rear_view_allowed = params.get("IsRearViewMirror") == "1"
 
   # 0.0 - 1.0
-  awareness_status = 0.
+  awareness_status = 1.
 
   rk = Ratekeeper(rate, print_delay_threshold=2./1000)
 
@@ -507,7 +497,7 @@ def controlsd_thread(gctx, rate=100):
     prof.checkpoint("Sample")
 
     # define plan
-    plan, plan_ts = calc_plan(CS, events, PL, LoC)
+    plan, plan_ts = calc_plan(CS, events, PL, LoC, v_cruise_kph, awareness_status)
     prof.checkpoint("Plan")
 
     if not passive:

selfdrive/controls/lib/adaptivecruise.py

@@ -1,294 +0,0 @@
-import math
-import numpy as np
-from common.numpy_fast import clip, interp
-import selfdrive.messaging as messaging
-
-# TODO: we compute a_pcm but we don't use it, as accelOverride is hardcoded in controlsd
-
-# lookup tables VS speed to determine min and max accels in cruise
-_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
-_A_CRUISE_MAX_V  = [1., 1., .8, .5, .30]
-_A_CRUISE_MAX_BP = [0.,  5., 10., 20., 40.]
-
-def calc_cruise_accel_limits(v_ego):
-  a_cruise_min = interp(v_ego, _A_CRUISE_MIN_BP, _A_CRUISE_MIN_V)
-  a_cruise_max = interp(v_ego, _A_CRUISE_MAX_BP, _A_CRUISE_MAX_V)
-  return np.vstack([a_cruise_min, a_cruise_max])
-
-_A_TOTAL_MAX_V = [1.5, 1.9, 3.2]
-_A_TOTAL_MAX_BP = [0., 20., 40.]
-
-def limit_accel_in_turns(v_ego, angle_steers, a_target, a_pcm, CP):
-  #*** this function returns a limited long acceleration allowed, depending on the existing lateral acceleration
-  # this should avoid accelerating when losing the target in turns
-  deg_to_rad = np.pi / 180.  # from can reading to rad
-
-  a_total_max = interp(v_ego, _A_TOTAL_MAX_BP, _A_TOTAL_MAX_V)
-  a_y = v_ego**2 * angle_steers * deg_to_rad / (CP.sR * CP.l)
-  a_x_allowed = math.sqrt(max(a_total_max**2 - a_y**2, 0.))
-
-  a_target[1] = min(a_target[1], a_x_allowed)
-  a_pcm = min(a_pcm, a_x_allowed)
-  return a_target, a_pcm
-
-def process_a_lead(a_lead):
-  # soft threshold of 0.5m/s^2 applied to a_lead to reject noise, also not considered positive a_lead
-  a_lead_threshold = 0.5
-  a_lead = min(a_lead + a_lead_threshold, 0)
-  return a_lead
-
-def calc_desired_distance(v_lead):
-  #*** compute desired distance ***
-  t_gap = 1.7  # good to be far away
-  d_offset = 4 # distance when at zero speed
-  return d_offset + v_lead * t_gap
-
-
-#linear slope
-_L_SLOPE_V = [0.40, 0.10]
-_L_SLOPE_BP = [0.,  40]
-
-# parabola slope
-_P_SLOPE_V = [1.0, 0.25]
-_P_SLOPE_BP = [0., 40]
-
-def calc_desired_speed(d_lead, d_des, v_lead, a_lead):
-  #*** compute desired speed ***
-  # the desired speed curve is divided in 4 portions: 
-  # 1-constant
-  # 2-linear to regain distance
-  # 3-linear to shorten distance
-  # 4-parabolic (constant decel)
-
-  max_runaway_speed = -2. # no slower than 2m/s over the lead
-
-  # interpolate the lookups to find the slopes for a give lead speed
-  l_slope = interp(v_lead, _L_SLOPE_BP, _L_SLOPE_V)
-  p_slope = interp(v_lead, _P_SLOPE_BP, _P_SLOPE_V)
-
-  # this is where parabola and linear curves are tangents  
-  x_linear_to_parabola = p_slope / l_slope**2
-
-  # parabola offset to have the parabola being tangent to the linear curve
-  x_parabola_offset = p_slope / (2 * l_slope**2)
-
-  if d_lead < d_des:
-    # calculate v_rel_des on the line that connects 0m at max_runaway_speed to d_des
-    v_rel_des_1 = (- max_runaway_speed) / d_des * (d_lead - d_des)
-    # calculate v_rel_des on one third of the linear slope
-    v_rel_des_2 = (d_lead - d_des) * l_slope / 3.
-    # take the min of the 2 above
-    v_rel_des = min(v_rel_des_1, v_rel_des_2)
-    v_rel_des = max(v_rel_des, max_runaway_speed)
-  elif d_lead < d_des + x_linear_to_parabola:
-    v_rel_des = (d_lead - d_des) * l_slope
-    v_rel_des = max(v_rel_des, max_runaway_speed)
-  else:
-    v_rel_des = math.sqrt(2 * (d_lead - d_des - x_parabola_offset) * p_slope)
-
-  # compute desired speed
-  v_target = v_rel_des + v_lead
-
-  # compute v_coast: above this speed we want to coast
-  t_lookahead = 1.   # how far in time we consider a_lead to anticipate the coast region
-  v_coast_shift = max(a_lead * t_lookahead, - v_lead)   # don't consider projections that would make v_lead<0
-  v_coast = (v_lead + v_target)/2 + v_coast_shift              # no accel allowed above this line
-  v_coast = min(v_coast, v_target)
-
-  return v_target, v_coast
-
-def calc_critical_decel(d_lead, v_rel, d_offset, v_offset):
-  # this function computes the required decel to avoid crashing, given safety offsets
-  a_critical =  - max(0., v_rel + v_offset)**2/max(2*(d_lead - d_offset), 0.5)
-  return a_critical
-
-
-# maximum acceleration adjustment
-_A_CORR_BY_SPEED_V = [0.4, 0.4, 0]
-# speeds
-_A_CORR_BY_SPEED_BP = [0., 2., 10.]
-
-# max acceleration allowed in acc, which happens in restart
-A_ACC_MAX = max(_A_CORR_BY_SPEED_V) + max(_A_CRUISE_MAX_V)
-
-def calc_positive_accel_limit(d_lead, d_des, v_ego, v_rel, v_ref, v_rel_ref, v_coast, v_target, a_lead_contr, a_max):
-  a_coast_min = -1.0   # never coast faster then -1m/s^2
-  # coasting behavior above v_coast. Forcing a_max to be negative will force the pid_speed to decrease,
-  # regardless v_target
-  if v_ref > min(v_coast, v_target):
-    # for smooth coast we can be aggressive and target a point where car would actually crash
-    v_offset_coast = 0.
-    d_offset_coast = d_des/2. - 4.
-
-    # acceleration value to smoothly coast until we hit v_target
-    if d_lead > d_offset_coast + 0.1:
-      a_coast = calc_critical_decel(d_lead, v_rel_ref, d_offset_coast, v_offset_coast)
-      # if lead is decelerating, then offset the coast decel
-      a_coast += a_lead_contr
-      a_max = max(a_coast, a_coast_min)
-    else:
-      a_max = a_coast_min
-  else:
-    # same as cruise accel, plus add a small correction based on relative lead speed
-    # if the lead car is faster, we can accelerate more, if the car is slower, then we can reduce acceleration
-    a_max = a_max + interp(v_ego, _A_CORR_BY_SPEED_BP, _A_CORR_BY_SPEED_V) \
-                  * clip(-v_rel / 4., -.5, 1)
-  return a_max
-
-# arbitrary limits to avoid too high accel being computed
-_A_SAT = [-10., 5.]
-
-# do not consider a_lead at 0m/s, fully consider it at 10m/s
-_A_LEAD_LOW_SPEED_V = [0., 1.]
-
-# speed break points
-_A_LEAD_LOW_SPEED_BP = [0., 10.]
-
-# add a small offset to the desired decel, just for safety margin
-_DECEL_OFFSET_V = [-0.3, -0.5, -0.5, -0.4, -0.3]
-
-# speed bp: different offset based on the likelyhood that lead decels abruptly
-_DECEL_OFFSET_BP = [0., 4., 15., 30, 40.]
-
-
-def calc_acc_accel_limits(d_lead, d_des, v_ego, v_pid, v_lead, v_rel, a_lead,
-                          v_target, v_coast, a_target, a_pcm):
-  #*** compute max accel ***
-  # v_rel is now your velocity in lead car frame
-  v_rel *= -1  # this simplifies things when thinking in d_rel-v_rel diagram
-
-  v_rel_pid = v_pid - v_lead
-
-  # this is how much lead accel we consider in assigning the desired decel
-  a_lead_contr = a_lead * interp(v_lead, _A_LEAD_LOW_SPEED_BP,
-                                 _A_LEAD_LOW_SPEED_V) * 0.8
-
-  # first call of calc_positive_accel_limit is used to shape v_pid
-  a_target[1] = calc_positive_accel_limit(d_lead, d_des, v_ego, v_rel, v_pid,
-                                          v_rel_pid, v_coast, v_target,
-                                          a_lead_contr, a_target[1])
-  # second call of calc_positive_accel_limit is used to limit the pcm throttle
-  # control (only useful when we don't control throttle directly)
-  a_pcm = calc_positive_accel_limit(d_lead, d_des, v_ego, v_rel, v_ego,
-                                    v_rel, v_coast, v_target,
-                                    a_lead_contr, a_pcm)
-
-  #*** compute max decel ***
-  v_offset = 1.  # assume the car is 1m/s slower
-  d_offset = 1.  # assume the distance is 1m lower
-  if v_target - v_ego > 0.5:
-    pass  # acc target speed is above vehicle speed, so we can use the cruise limits
-  elif d_lead > d_offset + 0.01:  # add small value to avoid by zero divisions
-    # compute needed accel to get to 1m distance with -1m/s rel speed
-    decel_offset = interp(v_lead, _DECEL_OFFSET_BP, _DECEL_OFFSET_V)
-
-    critical_decel = calc_critical_decel(d_lead, v_rel, d_offset, v_offset)
-    a_target[0] = min(decel_offset + critical_decel + a_lead_contr,
-                      a_target[0])
-  else:
-    a_target[0] = _A_SAT[0]
-  # a_min can't be higher than a_max
-  a_target[0] = min(a_target[0], a_target[1])
-  # final check on limits
-  a_target = np.clip(a_target, _A_SAT[0], _A_SAT[1])
-  a_target = a_target.tolist()
-  return a_target, a_pcm
-
-def calc_jerk_factor(d_lead, v_rel):
-  # we don't have an explicit jerk limit, so this function calculates a factor
-  # that is used by the PID controller to scale the gains. Not the cleanest solution 
-  # but we need this for the demo.
-  # TODO: Calculate Kp and Ki directly in this function.
-
-  # the higher is the decel required to avoid a crash, the higher is the PI factor scaling
-  d_offset = 0.5
-  v_offset = 2.
-  a_offset = 1.
-  jerk_factor_max = 1.0 # can't increase Kp and Ki more than double.
-  if d_lead < d_offset + 0.1: # add small value to avoid by zero divisions
-    jerk_factor = jerk_factor_max
-  else:
-    a_critical = - calc_critical_decel(d_lead, -v_rel, d_offset, v_offset)
-    # increase Kp and Ki by 20% for every 1m/s2 of decel required above 1m/s2
-    jerk_factor = max(a_critical - a_offset, 0.)/5.
-    jerk_factor = min(jerk_factor, jerk_factor_max)
-  return jerk_factor
-
-
-
-MAX_SPEED_POSSIBLE = 55.
-
-def compute_speed_with_leads(v_ego, angle_steers, v_pid, l1, l2, CP):
-  # drive limits
-  # TODO: Make lims function of speed (more aggressive at low speed).
-  a_lim = [-3., 1.5]
-
-  #*** set target speed pretty high, as lead hasn't been considered yet
-  v_target_lead = MAX_SPEED_POSSIBLE
-
-  #*** set accel limits as cruise accel/decel limits ***
-  a_target = calc_cruise_accel_limits(v_ego)
-
-  # start with 1
-  a_pcm = 1.
-
-  #*** limit max accel in sharp turns
-  a_target, a_pcm = limit_accel_in_turns(v_ego, angle_steers, a_target, a_pcm, CP)
-  jerk_factor = 0.
-
-  if l1 is not None and l1.status:
-    #*** process noisy a_lead signal from radar processing ***
-    a_lead_p = process_a_lead(l1.aLeadK)
-
-    #*** compute desired distance ***
-    d_des = calc_desired_distance(l1.vLead)
-
-    #*** compute desired speed ***
-    v_target_lead, v_coast = calc_desired_speed(l1.dRel, d_des, l1.vLead, a_lead_p)
-
-    if l2 is not None and l2.status:
-      #*** process noisy a_lead signal from radar processing ***
-      a_lead_p2 = process_a_lead(l2.aLeadK)
-
-      #*** compute desired distance ***
-      d_des2 = calc_desired_distance(l2.vLead)
-
-      #*** compute desired speed ***
-      v_target_lead2, v_coast2 = calc_desired_speed(l2.dRel, d_des2, l2.vLead, a_lead_p2)
-
-      # listen to lead that makes you go slower
-      if v_target_lead2 < v_target_lead:
-        l1 = l2
-        d_des, a_lead_p, v_target_lead, v_coast = d_des2, a_lead_p2, v_target_lead2, v_coast2
-
-    # l1 is the main lead now
-
-    #*** compute accel limits ***
-    a_target1, a_pcm1 = calc_acc_accel_limits(l1.dRel, d_des, v_ego, v_pid, l1.vLead,
-                                     l1.vRel, a_lead_p, v_target_lead, v_coast, a_target, a_pcm)
-
-    # we can now limit a_target to a_lim
-    a_target = np.clip(a_target1, a_lim[0], a_lim[1])
-    a_pcm = np.clip(a_pcm1, a_lim[0], a_lim[1]).tolist()
-
-    #*** compute max factor ***
-    jerk_factor = calc_jerk_factor(l1.dRel, l1.vRel)
-
-  # force coasting decel if driver hasn't been controlling car in a while
-  return v_target_lead, a_target, a_pcm, jerk_factor
-
-
-class AdaptiveCruise(object):
-  def __init__(self):
-    self.l1, self.l2 = None, None
-  def update(self, v_ego, angle_steers, v_pid, CP, l20):
-    if l20 is not None:
-      self.l1 = l20.live20.leadOne
-      self.l2 = l20.live20.leadTwo
-
-    self.v_target_lead, self.a_target, self.a_pcm, self.jerk_factor = \
-      compute_speed_with_leads(v_ego, angle_steers, v_pid, self.l1, self.l2, CP)
-    self.has_lead = self.v_target_lead != MAX_SPEED_POSSIBLE

selfdrive/controls/lib/alertmanager.py

@@ -59,9 +59,9 @@ class AlertManager(object):
         PT.MID, None, "beepSingle", .2, 0., 0.),
 
     "fcw": Alert(
-        "", 
-        "", 
-        PT.LOW, None, None, .1, .1, .1),
+        "Brake", 
+        "Risk of Collision", 
+        PT.HIGH, "fcw", "chimeRepeated", 1., 2., 2.),
 
     "steerSaturated": Alert(
         "Take Control", 

selfdrive/controls/lib/drive_helpers.py

@@ -33,7 +33,7 @@ def rate_limit(new_value, last_value, dw_step, up_step):
   return clip(new_value, last_value + dw_step, last_value + up_step)
 
 
-def learn_angle_offset(lateral_control, v_ego, angle_offset, c_poly, c_prob, y_des, steer_override):
+def learn_angle_offset(lateral_control, v_ego, angle_offset, c_poly, c_prob, angle_steers, steer_override):
   # simple integral controller that learns how much steering offset to put to have the car going straight
   # while being in the middle of the lane
   min_offset = -5.  # deg
@@ -42,7 +42,7 @@ def learn_angle_offset(lateral_control, v_ego, angle_offset, c_poly, c_prob, y_d
   min_learn_speed = 1.
 
   # learn less at low speed or when turning
-  alpha_v = alpha * c_prob * (max(v_ego - min_learn_speed, 0.)) / (1. + 0.5*abs(y_des))
+  alpha_v = alpha * c_prob * (max(v_ego - min_learn_speed, 0.)) / (1. + 0.2 * abs(angle_steers))
 
   # only learn if lateral control is active and if driver is not overriding:
   if lateral_control and not steer_override:

selfdrive/controls/lib/fcw.py

@@ -1,66 +0,0 @@
-import numpy as np
-from common.realtime import sec_since_boot
-
-#Time to collisions greater than 5s are iognored
-MAX_TTC = 5.
-
-def calc_ttc(l1):
-  # if l1 is None, return max ttc immediately
-  if not l1:
-    return MAX_TTC
-  # this function returns the time to collision (ttc), assuming that
-  # ARel will stay constant TODO: review this assumptions
-  # change sign to rel quantities as it's going to be easier for calculations
-  vRel = -l1.vRel
-  aRel = -l1.aRel
-
-  # assuming that closing gap ARel comes from lead vehicle decel,
-  # then limit ARel so that v_lead will get to zero in no sooner than t_decel.
-  # This helps underweighting ARel when v_lead is close to zero.
-  t_decel = 2.
-  aRel = np.minimum(aRel, l1.vLead/t_decel)
-
-  # delta of the quadratic equation to solve for ttc
-  delta = vRel**2 + 2 * l1.dRel * aRel
-
-  # assign an arbitrary high ttc value if there is no solution to ttc
-  if delta < 0.1 or (np.sqrt(delta) + vRel < 0.1):
-    ttc = MAX_TTC
-  else:
-    ttc = np.minimum(2 * l1.dRel / (np.sqrt(delta) + vRel), MAX_TTC)
-  return ttc
-
-class ForwardCollisionWarning(object):
-  def __init__(self, dt):
-    self.last_active = 0.
-    self.violation_time = 0.
-    self.active = False
-    self.dt = dt   # time step
-
-  def process(self, CS, AC):
-    # send an fcw alert if the violation time > violation_thrs
-    violation_thrs = 0.3  # fcw turns on after a continuous violation for this time
-    fcw_t_delta = 5.         # no more than one fcw alert within this time
-    a_acc_on  = -2.0         # with system on, above this limit of desired decel, we should trigger fcw
-    a_acc_off = -2.5         # with system off, above this limit of desired decel, we should trigger fcw
-    ttc_thrs = 2.5           # ttc threshold for fcw
-    v_fcw_min = 5.           # no fcw below 5m/s
-    steer_angle_th = 40.     # deg, no fcw above this steer angle
-    cur_time = sec_since_boot()
-
-    ttc = calc_ttc(AC.l1)
-    a_fcw = a_acc_on if CS.cruiseState.enabled else a_acc_off
-
-    # increase violation time if we want to decelerate quite fast
-    if AC.l1 and ( \
-        (CS.vEgo > v_fcw_min) and (CS.vEgo > AC.v_target_lead) and (AC.a_target[0] < a_fcw) \
-        and not CS.brakePressed and ttc < ttc_thrs and abs(CS.steeringAngle) < steer_angle_th \
-        and AC.l1.fcw):
-      self.violation_time = np.minimum(self.violation_time + self.dt, violation_thrs)
-    else:
-      self.violation_time = np.maximum(self.violation_time - 2*self.dt, 0)
-
-    # fire FCW
-    self.active = self.violation_time >= violation_thrs and cur_time > (self.last_active + fcw_t_delta)
-    if self.active:
-      self.last_active = cur_time

selfdrive/controls/lib/latcontrol.py

@@ -2,11 +2,14 @@ import math
 from selfdrive.controls.lib.pid import PIController
 from selfdrive.controls.lib.lateral_mpc import libmpc_py
 from common.numpy_fast import clip, interp
-
+from common.realtime import sec_since_boot
 
 # 100ms is a rule of thumb estimation of lag from image processing to actuator command
 ACTUATORS_DELAY = 0.1
 
+_DT = 0.01    # 100Hz
+_DT_MPC = 0.05  # 20Hz
+
 
 def calc_states_after_delay(states, v_ego, steer_angle, curvature_factor, steer_ratio):
   states[0].x = v_ego * ACTUATORS_DELAY
@@ -23,8 +26,6 @@ class LatControl(object):
     self.pid = PIController(VM.CP.steerKp, VM.CP.steerKi, k_f=VM.CP.steerKf, pos_limit=1.0)
     self.setup_mpc()
 
-    self.y_des = -1  # Legacy
-
   def setup_mpc(self):
     self.libmpc = libmpc_py.libmpc
     self.libmpc.init()
@@ -38,15 +39,20 @@ class LatControl(object):
     self.cur_state[0].delta = 0.0
 
     self.last_mpc_ts = 0.0
-    self.angle_steers_des = 0
+    self.angle_steers_des = 0.0
+    self.angle_steers_des_mpc = 0.0
+    self.angle_steers_des_prev = 0.0
+    self.angle_steers_des_time = 0.0
 
   def reset(self):
     self.pid.reset()
 
   def update(self, active, v_ego, angle_steers, steer_override, d_poly, angle_offset, VM, PL):
+    cur_time = sec_since_boot()
     self.mpc_updated = False
-    if self.last_mpc_ts + 0.001 < PL.last_md_ts:
+    if self.last_mpc_ts < PL.last_md_ts:
       self.last_mpc_ts = PL.last_md_ts
+      self.angle_steers_des_prev = self.angle_steers_des_mpc
 
       curvature_factor = VM.curvature_factor(v_ego)
 
@@ -65,16 +71,19 @@ class LatControl(object):
       delta_desired = self.mpc_solution[0].delta[1]
       self.cur_state[0].delta = delta_desired
 
-      self.angle_steers_des = float(math.degrees(delta_desired * VM.CP.sR) + angle_offset)
+      self.angle_steers_des_mpc = float(math.degrees(delta_desired * VM.CP.sR) + angle_offset)
+      self.angle_steers_des_time = cur_time
       self.mpc_updated = True
 
     if v_ego < 0.3 or not active:
       output_steer = 0.0
       self.pid.reset()
     else:
-      steer_max = get_steer_max(VM.CP, v_ego)
-      self.pid.pos_limit = steer_max
-      self.pid.neg_limit = -steer_max
+      dt = min(cur_time - self.angle_steers_des_time, _DT_MPC + _DT) + _DT  # no greater than dt mpc + dt, to prevent too high extraps
+      self.angle_steers_des = self.angle_steers_des_prev + (dt / _DT_MPC) * (self.angle_steers_des_mpc - self.angle_steers_des_prev)
+      steers_max = get_steer_max(VM.CP, v_ego)
+      self.pid.pos_limit = steers_max
+      self.pid.neg_limit = -steers_max
       steer_feedforward = self.angle_steers_des * v_ego**2  # proportional to realigning tire momentum (~ lateral accel)
       output_steer = self.pid.update(self.angle_steers_des, angle_steers, check_saturation=(v_ego > 10), override=steer_override, feedforward=steer_feedforward)
 

selfdrive/controls/lib/longcontrol.py

@@ -4,7 +4,8 @@ from selfdrive.config import Conversions as CV
 from selfdrive.controls.lib.pid import PIController
 
 STOPPING_EGO_SPEED = 0.5
-STOPPING_TARGET_SPEED = 0.3
+MIN_CAN_SPEED = 0.3
+STOPPING_TARGET_SPEED = MIN_CAN_SPEED + 0.01
 STARTING_TARGET_SPEED = 0.5
 BRAKE_THRESHOLD_TO_PID = 0.2
 
@@ -18,12 +19,16 @@ class LongCtrlState:
   starting = 'starting'  # starting and releasing brakes in open loop before giving back to PID
 
 
+
 def long_control_state_trans(active, long_control_state, v_ego, v_target, v_pid,
-                             output_gb, brake_pressed):
+                             output_gb, brake_pressed, cruise_standstill):
+
+  stopping_condition = (v_ego < 2.0 and cruise_standstill) or \
+                       (v_ego < STOPPING_EGO_SPEED and \
+                        ((v_pid < STOPPING_TARGET_SPEED and v_target < STOPPING_TARGET_SPEED) or
+                        brake_pressed))
 
-  stopping_condition = (v_ego < STOPPING_EGO_SPEED) and \
-                       (((v_pid < STOPPING_TARGET_SPEED) and (v_target < STOPPING_TARGET_SPEED)) or
-                        (brake_pressed))
+  starting_condition = v_target > STARTING_TARGET_SPEED and not cruise_standstill
 
   if not active:
     long_control_state = LongCtrlState.off
@@ -38,7 +43,7 @@ def long_control_state_trans(active, long_control_state, v_ego, v_target, v_pid,
         long_control_state = LongCtrlState.stopping
 
     elif long_control_state == LongCtrlState.stopping:
-      if (v_target > STARTING_TARGET_SPEED):
+      if starting_condition:
         long_control_state = LongCtrlState.starting
 
     elif long_control_state == LongCtrlState.starting:
@@ -50,15 +55,8 @@ def long_control_state_trans(active, long_control_state, v_ego, v_target, v_pid,
   return long_control_state
 
 
-_KP_BP = [0., 5., 35.]
-_KP_V = [1.2, 0.8, 0.5]
-
-_KI_BP = [0., 35.]
-_KI_V = [0.18, 0.12]
-
 stopping_brake_rate = 0.2  # brake_travel/s while trying to stop
 starting_brake_rate = 0.8  # brake_travel/s while releasing on restart
-starting_Ui = 0.5  # Since we don't have much info about acceleration at this point, be conservative
 brake_stopping_target = 0.5  # apply at least this amount of brake to maintain the vehicle stationary
 
 _MAX_SPEED_ERROR_BP = [0., 30.]  # speed breakpoints
@@ -66,10 +64,10 @@ _MAX_SPEED_ERROR_V = [1.5, .8]  # max positive v_pid error VS actual speed; this
 
 
 class LongControl(object):
-  def __init__(self, compute_gb):
+  def __init__(self, CP, compute_gb):
     self.long_control_state = LongCtrlState.off  # initialized to off
-    self.pid = PIController((_KP_BP, _KP_V),
-                            (_KI_BP, _KI_V),
+    self.pid = PIController((CP.longitudinalKpBP, CP.longitudinalKpV),
+                            (CP.longitudinalKiBP, CP.longitudinalKiV),
                             rate=100.0,
                             sat_limit=0.8,
                             convert=compute_gb)
@@ -80,27 +78,18 @@ class LongControl(object):
     self.pid.reset()
     self.v_pid = v_pid
 
-  def update(self, active, v_ego, brake_pressed, standstill, v_cruise, v_target_lead, a_target,
-             jerk_factor, CP):
-
+  def update(self, active, v_ego, brake_pressed, standstill, cruise_standstill, v_cruise, v_target, v_target_future, a_target, CP, lead_1):
     # actuation limits
     gas_max = interp(v_ego, CP.gasMaxBP, CP.gasMaxV)
     brake_max = interp(v_ego, CP.brakeMaxBP, CP.brakeMaxV)
 
-    overshoot_allowance = 2.0  # overshoot allowed when changing accel sign
-
     output_gb = self.last_output_gb
-
-    # limit max target speed based on cruise setting
-    v_target = min(v_target_lead, v_cruise * CV.KPH_TO_MS)
     rate = 100.0
-    max_speed_delta_up = a_target[1] * 1.0 / rate
-    max_speed_delta_down = a_target[0] * 1.0 / rate
-
-    self.long_control_state = long_control_state_trans(active, self.long_control_state, v_ego,\
-                                                       v_target, self.v_pid, output_gb, brake_pressed)
+    self.long_control_state = long_control_state_trans(active, self.long_control_state, v_ego,
+                                                       v_target_future, self.v_pid, output_gb,
+                                                       brake_pressed, cruise_standstill)
 
-    v_ego_pid = max(v_ego, 0.3)  # Without this we get jumps, CAN bus reports 0 when speed < 0.3
+    v_ego_pid = max(v_ego, MIN_CAN_SPEED)  # Without this we get jumps, CAN bus reports 0 when speed < 0.3
 
     # *** long control behavior based on state
     if self.long_control_state == LongCtrlState.off:
@@ -110,29 +99,15 @@ class LongControl(object):
 
     # tracking objects and driving
     elif self.long_control_state == LongCtrlState.pid:
-      #reset v_pid close to v_ego if it was too far and new v_target is closer to v_ego
-      if ((self.v_pid > v_ego + overshoot_allowance) and (v_target < self.v_pid)):
-        self.v_pid = max(v_target, v_ego + overshoot_allowance)
-      elif ((self.v_pid < v_ego - overshoot_allowance) and (v_target > self.v_pid)):
-        self.v_pid = min(v_target, v_ego - overshoot_allowance)
-
-      # move v_pid no faster than allowed accel limits
-      if (v_target > self.v_pid + max_speed_delta_up):
-        self.v_pid += max_speed_delta_up
-      elif (v_target < self.v_pid + max_speed_delta_down):
-        self.v_pid += max_speed_delta_down
-      else:
-        self.v_pid = v_target
-
-      # to avoid too much wind up on acceleration, limit positive speed error
-      if CP.enableGas:
-        max_speed_error = interp(v_ego, _MAX_SPEED_ERROR_BP, _MAX_SPEED_ERROR_V)
-        self.v_pid = min(self.v_pid, v_ego + max_speed_error)
+      prevent_overshoot = not CP.stoppingControl and v_ego < 1.5 and v_target_future < 0.7
 
+      self.v_pid = max(v_target, MIN_CAN_SPEED)
       self.pid.pos_limit = gas_max
       self.pid.neg_limit = - brake_max
       deadzone = interp(v_ego_pid, CP.longPidDeadzoneBP, CP.longPidDeadzoneV)
-      output_gb = self.pid.update(self.v_pid, v_ego_pid, speed=v_ego_pid, jerk_factor=jerk_factor, deadzone=deadzone)
+      output_gb = self.pid.update(self.v_pid, v_ego_pid, speed=v_ego_pid, deadzone=deadzone, feedforward=a_target, freeze_integrator=prevent_overshoot)
+      if prevent_overshoot:
+        output_gb = min(output_gb, 0.0)
 
     # intention is to stop, switch to a different brake control until we stop
     elif self.long_control_state == LongCtrlState.stopping:
@@ -150,7 +125,6 @@ class LongControl(object):
         output_gb += starting_brake_rate / rate
       self.v_pid = v_ego
       self.pid.reset()
-      self.pid.i = starting_Ui
 
     self.last_output_gb = output_gb
     final_gas = clip(output_gb, 0., gas_max)

selfdrive/controls/lib/longitudinal_mpc/Makefile

@@ -0,0 +1,94 @@
+CC = clang
+CXX = clang++
+
+PHONELIBS = ../../../../phonelibs
+
+UNAME_M := $(shell uname -m)
+
+CFLAGS = -O3 -fPIC -I.
+CXXFLAGS = -O3 -fPIC -I.
+
+QPOASES_FLAGS = -I$(PHONELIBS)/qpoases -I$(PHONELIBS)/qpoases/INCLUDE -I$(PHONELIBS)/qpoases/SRC
+
+ACADO_FLAGS = -I$(PHONELIBS)/acado/include -I$(PHONELIBS)/acado/include/acado
+
+ifeq ($(UNAME_M),aarch64)
+ACADO_LIBS := -L $(PHONELIBS)/acado/aarch64/lib -l:libacado_toolkit.a -l:libacado_casadi.a -l:libacado_csparse.a
+else
+ACADO_LIBS := -L $(PHONELIBS)/acado/x64/lib -l:libacado_toolkit.a -l:libacado_casadi.a -l:libacado_csparse.a
+endif
+
+OBJS = \
+  qp/Bounds.o \
+  qp/Constraints.o \
+  qp/CyclingManager.o \
+  qp/Indexlist.o \
+  qp/MessageHandling.o \
+  qp/QProblem.o \
+  qp/QProblemB.o \
+  qp/SubjectTo.o \
+  qp/Utils.o \
+  qp/EXTRAS/SolutionAnalysis.o \
+	mpc_export/acado_qpoases_interface.o \
+  mpc_export/acado_integrator.o \
+  mpc_export/acado_solver.o \
+  mpc_export/acado_auxiliary_functions.o \
+  mpc.o
+
+DEPS := $(OBJS:.o=.d)
+
+.PHONY: all
+all: libcommampc1.so libcommampc2.so
+
+libcommampc1.so: $(OBJS)
+	$(CXX) -shared -o '$@' $^ -lm
+
+libcommampc2.so: libcommampc1.so
+	cp libcommampc1.so libcommampc2.so
+
+qp/%.o: $(PHONELIBS)/qpoases/SRC/%.cpp
+	@echo "[ CXX ] $@"
+	mkdir -p qp
+	$(CXX) $(CXXFLAGS) -MMD \
+           -I mpc_export/ \
+           $(QPOASES_FLAGS) \
+           -c -o '$@' '$<'
+
+qp/EXTRAS/%.o: $(PHONELIBS)/qpoases/SRC/EXTRAS/%.cpp
+	@echo "[ CXX ] $@"
+	mkdir -p qp/EXTRAS
+	$(CXX) $(CXXFLAGS) -MMD \
+           -I mpc_export/ \
+           $(QPOASES_FLAGS) \
+           -c -o '$@' '$<'
+
+%.o: %.cpp
+	@echo "[ CXX ] $@"
+	$(CXX) $(CXXFLAGS) -MMD \
+           -I mpc_export/ \
+           $(QPOASES_FLAGS) \
+           -c -o '$@' '$<'
+
+%.o: %.c
+	@echo "[ CC ] $@"
+	$(CC) $(CFLAGS) -MMD \
+           -I mpc_export/ \
+           $(QPOASES_FLAGS) \
+           -c -o '$@' '$<'
+
+generator: generator.cpp
+	$(CXX) -Wall -std=c++11 \
+         generator.cpp \
+         -o generator \
+         $(ACADO_FLAGS) \
+         $(ACADO_LIBS)
+
+.PHONY: generate
+generate: generator
+	./generator
+
+.PHONY: clean
+clean:
+	rm -f libcommampc1.so libcommampc2.so generator $(OBJS) $(DEPS)
+
+-include $(DEPS)

selfdrive/controls/lib/longitudinal_mpc/generator.cpp

@@ -0,0 +1,98 @@
+#include <acado_code_generation.hpp>
+
+const int controlHorizon = 50;
+const double samplingTime = 0.2;
+
+using namespace std;
+
+#define G 9.81
+#define TR 1.8
+
+#define RW(v_ego, v_l) (v_ego * TR - (v_l - v_ego) * TR + v_ego*v_ego/(2*G) - v_l*v_l / (2*G))
+#define NORM_RW_ERROR(v_ego, v_l, p) ((RW(v_ego, v_l) + 4.0 - p)/(sqrt(v_ego + 0.5) + 0.1))
+
+int main( )
+{
+  USING_NAMESPACE_ACADO
+
+
+  DifferentialEquation f;
+
+  DifferentialState x_ego, v_ego, a_ego;
+  DifferentialState x_l, v_l, a_l;
+
+  OnlineData lambda;
+
+  Control j_ego;
+
+  auto desired = 4.0 + RW(v_ego, v_l);
+  auto d_l = x_l - x_ego;
+
+  // Equations of motion
+  f << dot(x_ego) == v_ego;
+  f << dot(v_ego) == a_ego;
+  f << dot(a_ego) == j_ego;
+
+  f << dot(x_l) == v_l;
+  f << dot(v_l) == a_l;
+  f << dot(a_l) == -lambda * a_l;
+
+  // Running cost
+  Function h;
+  h << exp(0.3 * NORM_RW_ERROR(v_ego, v_l, d_l)) - exp(0.3 * NORM_RW_ERROR(v_ego, v_l, desired));
+  h << (d_l - desired) / (0.1 * v_ego + 0.5);
+  h << a_ego * (1.0 + v_ego / 10.0);
+  h << j_ego * (1.0 + v_ego / 10.0);
+
+  DMatrix Q(4,4);
+  Q(0,0) = 5.0;
+  Q(1,1) = 0.1;
+  Q(2,2) = 10.0;
+  Q(3,3) = 20.0;
+
+  // Terminal cost
+  Function hN;
+  hN << exp(0.3 * NORM_RW_ERROR(v_ego, v_l, d_l)) - exp(0.3 * NORM_RW_ERROR(v_ego, v_l, desired));
+  hN << (d_l - desired) / (0.1 * v_ego + 0.5);
+  hN << a_ego * (1.0 + v_ego / 10.0);
+
+  DMatrix QN(3,3);
+  QN(0,0) = 5.0;
+  QN(1,1) = 0.1;
+  QN(2,2) = 10.0;
+
+  // Setup Optimal Control Problem
+  const double tStart = 0.0;
+  const double tEnd   = samplingTime * controlHorizon;
+
+  OCP ocp( tStart, tEnd, controlHorizon );
+  ocp.subjectTo(f);
+
+  ocp.minimizeLSQ(Q, h);
+  ocp.minimizeLSQEndTerm(QN, hN);
+
+  ocp.subjectTo( 0.0 <= v_ego);
+  ocp.setNOD(1);
+
+  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, 1 * controlHorizon);
+  mpc.set( MAX_NUM_QP_ITERATIONS, 500);
+
+  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( "mpc_export" ) != SUCCESSFUL_RETURN)
+    exit( EXIT_FAILURE );
+
+  mpc.printDimensionsQP( );
+
+  return EXIT_SUCCESS;
+}

selfdrive/controls/lib/longitudinal_mpc/libmpc_py.py

@@ -0,0 +1,43 @@
+import os
+import subprocess
+
+from cffi import FFI
+
+mpc_dir = os.path.join(os.path.dirname(os.path.abspath(__file__)))
+subprocess.check_output(["make", "-j4"], cwd=mpc_dir)
+
+
+def _get_libmpc(mpc_id):
+    libmpc_fn = os.path.join(mpc_dir, "libcommampc%d.so" % mpc_id)
+
+    ffi = FFI()
+    ffi.cdef("""
+    typedef struct {
+    double x_ego, v_ego, a_ego, x_l, v_l, a_l;
+    } state_t;
+
+
+    typedef struct {
+    double x_ego[50];
+    double v_ego[50];
+    double a_ego[50];
+    double j_ego[50];
+    double x_l[50];
+    double v_l[50];
+    double a_l[50];
+    } log_t;
+
+    void init();
+    void init_with_simulation(double v_ego, double x_l, double v_l, double a_l, double l);
+    int run_mpc(state_t * x0, log_t * solution,
+                double l);
+    """)
+
+    return (ffi, ffi.dlopen(libmpc_fn))
+
+
+mpcs = [_get_libmpc(1), _get_libmpc(2)]
+
+
+def get_libmpc(mpc_id):
+    return mpcs[mpc_id - 1]

selfdrive/controls/lib/longitudinal_mpc/mpc.c

@@ -0,0 +1,118 @@
+#include "acado_common.h"
+#include "acado_auxiliary_functions.h"
+
+#include <stdio.h>
+
+#define NX          ACADO_NX  /* Number of differential state variables.  */
+#define NXA         ACADO_NXA /* Number of algebraic variables. */
+#define NU          ACADO_NU  /* Number of control inputs. */
+#define NOD         ACADO_NOD  /* Number of online data values. */
+
+#define NY          ACADO_NY  /* Number of measurements/references on nodes 0..N - 1. */
+#define NYN         ACADO_NYN /* Number of measurements/references on node N. */
+
+#define N           ACADO_N   /* Number of intervals in the horizon. */
+
+ACADOvariables acadoVariables;
+ACADOworkspace acadoWorkspace;
+
+typedef struct {
+  double x_ego, v_ego, a_ego, x_l, v_l, a_l;
+} state_t;
+
+
+typedef struct {
+  double x_ego[N];
+  double v_ego[N];
+  double a_ego[N];
+  double j_ego[N];
+	double x_l[N];
+	double v_l[N];
+	double a_l[N];
+} log_t;
+
+void init(){
+  acado_initializeSolver();
+  int    i;
+
+  /* Initialize the states and controls. */
+  for (i = 0; i < NX * (N + 1); ++i)  acadoVariables.x[ i ] = 0.0;
+  for (i = 0; i < NU * N; ++i)  acadoVariables.u[ i ] = 0.0;
+
+  /* Initialize the measurements/reference. */
+  for (i = 0; i < NY * N; ++i)  acadoVariables.y[ i ] = 0.0;
+  for (i = 0; i < NYN; ++i)  acadoVariables.yN[ i ] = 0.0;
+
+  /* MPC: initialize the current state feedback. */
+  for (i = 0; i < NX; ++i) acadoVariables.x0[ i ] = 0.0;
+}
+
+void init_with_simulation(double v_ego, double x_l, double v_l, double a_l, double l){
+  int i;
+  double x_ego = 0.0;
+  double a_ego = 0.0;
+
+  if (v_ego > v_l){
+    a_ego = -(v_ego - v_l) * (v_ego - v_l) / (2.0 * x_l + 0.01) + a_l;
+  }
+  double dt = 0.2;
+
+  for (i = 0; i < N + 1; ++i){
+    acadoVariables.x[i*NX] = x_ego;
+    acadoVariables.x[i*NX+1] = v_ego;
+    acadoVariables.x[i*NX+2] = a_ego;
+
+    acadoVariables.x[i*NX+3] = x_l;
+    acadoVariables.x[i*NX+4] = v_l;
+    acadoVariables.x[i*NX+5] = a_l;
+
+    x_ego += v_ego * dt;
+    v_ego += a_ego * dt;
+
+    x_l += v_l * dt;
+    v_l += a_l * dt;
+    a_l += -l * a_l * dt;
+
+    if (v_ego <= 0.0) {
+      v_ego = 0.0;
+      a_ego = 0.0;
+    }
+  }
+  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 l){
+  int i;
+
+  for (i = 0; i <= NOD * N; i+= NOD){
+    acadoVariables.od[i] = l;
+  }
+
+  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] = x0->x_l;
+  acadoVariables.x[4] = acadoVariables.x0[4] = x0->v_l;
+  acadoVariables.x[5] = acadoVariables.x0[5] = x0->a_l;
+
+  acado_preparationStep();
+  acado_feedbackStep();
+
+	for (i = 0; i <= N; i++){
+    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->x_l[i] = acadoVariables.x[i*NX+3];
+		solution->v_l[i] = acadoVariables.x[i*NX+4];
+		solution->a_l[i] = acadoVariables.x[i*NX+5];
+
+    solution->j_ego[i] = acadoVariables.u[i];
+	}
+
+  // Dont shift states here. Current solution is closer to next timestep than if
+  // we shift by 0.2 seconds.
+
+  return acado_getNWSR();
+}

selfdrive/controls/lib/longitudinal_mpc/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/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/mpc_export/acado_common.h

@@ -0,0 +1,349 @@
+/*
+ *    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 50
+/** Number of online data values. */
+#define ACADO_NOD 1
+/** Number of path constraints. */
+#define ACADO_NPAC 0
+/** Number of control variables. */
+#define ACADO_NU 1
+/** Number of differential variables. */
+#define ACADO_NX 6
+/** 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 4
+/** 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 56
+/** Number of integration steps per shooting interval. */
+#define ACADO_RK_NIS 1
+/** 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 0
+
+
+/*
+ * 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: 51 x 6 (row major format)
+ * 
+ *  Matrix containing 51 differential variable vectors.
+ */
+real_t x[ 306 ];
+
+/** Column vector of size: 50
+ * 
+ *  Matrix containing 50 control variable vectors.
+ */
+real_t u[ 50 ];
+
+/** Column vector of size: 51
+ * 
+ *  Matrix containing 51 online data vectors.
+ */
+real_t od[ 51 ];
+
+/** Column vector of size: 200
+ * 
+ *  Matrix containing 50 reference/measurement vectors of size 4 for first 50 nodes.
+ */
+real_t y[ 200 ];
+
+/** Column vector of size: 3
+ * 
+ *  Reference/measurement vector for the 51. node.
+ */
+real_t yN[ 3 ];
+
+/** Column vector of size: 6
+ * 
+ *  Current state feedback vector.
+ */
+real_t x0[ 6 ];
+
+
+} 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: 50 */
+real_t rk_xxx[ 50 ];
+
+/** Matrix of size: 4 x 48 (row major format) */
+real_t rk_kkk[ 192 ];
+
+/** Row vector of size: 50 */
+real_t state[ 50 ];
+
+/** Column vector of size: 300 */
+real_t d[ 300 ];
+
+/** Column vector of size: 200 */
+real_t Dy[ 200 ];
+
+/** Column vector of size: 3 */
+real_t DyN[ 3 ];
+
+/** Matrix of size: 300 x 6 (row major format) */
+real_t evGx[ 1800 ];
+
+/** Column vector of size: 300 */
+real_t evGu[ 300 ];
+
+/** Column vector of size: 32 */
+real_t objAuxVar[ 32 ];
+
+/** Row vector of size: 8 */
+real_t objValueIn[ 8 ];
+
+/** Row vector of size: 32 */
+real_t objValueOut[ 32 ];
+
+/** Matrix of size: 300 x 6 (row major format) */
+real_t Q1[ 1800 ];
+
+/** Matrix of size: 300 x 4 (row major format) */
+real_t Q2[ 1200 ];
+
+/** Column vector of size: 50 */
+real_t R1[ 50 ];
+
+/** Matrix of size: 50 x 4 (row major format) */
+real_t R2[ 200 ];
+
+/** Column vector of size: 300 */
+real_t S1[ 300 ];
+
+/** Matrix of size: 6 x 6 (row major format) */
+real_t QN1[ 36 ];
+
+/** Matrix of size: 6 x 3 (row major format) */
+real_t QN2[ 18 ];
+
+/** Column vector of size: 6 */
+real_t Dx0[ 6 ];
+
+/** Matrix of size: 6 x 6 (row major format) */
+real_t T[ 36 ];
+
+/** Column vector of size: 7650 */
+real_t E[ 7650 ];
+
+/** Column vector of size: 7650 */
+real_t QE[ 7650 ];
+
+/** Matrix of size: 300 x 6 (row major format) */
+real_t QGx[ 1800 ];
+
+/** Column vector of size: 300 */
+real_t Qd[ 300 ];
+
+/** Column vector of size: 306 */
+real_t QDy[ 306 ];
+
+/** Matrix of size: 50 x 6 (row major format) */
+real_t H10[ 300 ];
+
+/** Matrix of size: 56 x 56 (row major format) */
+real_t H[ 3136 ];
+
+/** Matrix of size: 50 x 56 (row major format) */
+real_t A[ 2800 ];
+
+/** Column vector of size: 56 */
+real_t g[ 56 ];
+
+/** Column vector of size: 56 */
+real_t lb[ 56 ];
+
+/** Column vector of size: 56 */
+real_t ub[ 56 ];
+
+/** Column vector of size: 50 */
+real_t lbA[ 50 ];
+
+/** Column vector of size: 50 */
+real_t ubA[ 50 ];
+
+/** Column vector of size: 56 */
+real_t x[ 56 ];
+
+/** Column vector of size: 106 */
+real_t y[ 106 ];
+
+
+} 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.
+ *
+ *  \return Status code of the integrator.
+ */
+int acado_integrate( real_t* const rk_eta, int resetIntegrator );
+
+/** 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 51 with xEnd. 2. Initialize node 51 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/mpc_export/acado_integrator.c

@@ -0,0 +1,383 @@
+/*
+ *    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 + 48;
+const real_t* od = in + 49;
+
+/* Compute outputs: */
+out[0] = xd[1];
+out[1] = xd[2];
+out[2] = u[0];
+out[3] = xd[4];
+out[4] = xd[5];
+out[5] = (((real_t)(0.0000000000000000e+00)-od[0])*xd[5]);
+out[6] = xd[12];
+out[7] = xd[13];
+out[8] = xd[14];
+out[9] = xd[15];
+out[10] = xd[16];
+out[11] = xd[17];
+out[12] = xd[18];
+out[13] = xd[19];
+out[14] = xd[20];
+out[15] = xd[21];
+out[16] = xd[22];
+out[17] = xd[23];
+out[18] = (real_t)(0.0000000000000000e+00);
+out[19] = (real_t)(0.0000000000000000e+00);
+out[20] = (real_t)(0.0000000000000000e+00);
+out[21] = (real_t)(0.0000000000000000e+00);
+out[22] = (real_t)(0.0000000000000000e+00);
+out[23] = (real_t)(0.0000000000000000e+00);
+out[24] = xd[30];
+out[25] = xd[31];
+out[26] = xd[32];
+out[27] = xd[33];
+out[28] = xd[34];
+out[29] = xd[35];
+out[30] = xd[36];
+out[31] = xd[37];
+out[32] = xd[38];
+out[33] = xd[39];
+out[34] = xd[40];
+out[35] = xd[41];
+out[36] = (((real_t)(0.0000000000000000e+00)-od[0])*xd[36]);
+out[37] = (((real_t)(0.0000000000000000e+00)-od[0])*xd[37]);
+out[38] = (((real_t)(0.0000000000000000e+00)-od[0])*xd[38]);
+out[39] = (((real_t)(0.0000000000000000e+00)-od[0])*xd[39]);
+out[40] = (((real_t)(0.0000000000000000e+00)-od[0])*xd[40]);
+out[41] = (((real_t)(0.0000000000000000e+00)-od[0])*xd[41]);
+out[42] = xd[43];
+out[43] = xd[44];
+out[44] = (real_t)(1.0000000000000000e+00);
+out[45] = xd[46];
+out[46] = xd[47];
+out[47] = (((real_t)(0.0000000000000000e+00)-od[0])*xd[47]);
+}
+
+/* Fixed step size:0.2 */
+int acado_integrate( real_t* const rk_eta, int resetIntegrator )
+{
+int error;
+
+int run1;
+acadoWorkspace.rk_ttt = 0.0000000000000000e+00;
+rk_eta[6] = 1.0000000000000000e+00;
+rk_eta[7] = 0.0000000000000000e+00;
+rk_eta[8] = 0.0000000000000000e+00;
+rk_eta[9] = 0.0000000000000000e+00;
+rk_eta[10] = 0.0000000000000000e+00;
+rk_eta[11] = 0.0000000000000000e+00;
+rk_eta[12] = 0.0000000000000000e+00;
+rk_eta[13] = 1.0000000000000000e+00;
+rk_eta[14] = 0.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] = 0.0000000000000000e+00;
+rk_eta[20] = 1.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] = 1.0000000000000000e+00;
+rk_eta[28] = 0.0000000000000000e+00;
+rk_eta[29] = 0.0000000000000000e+00;
+rk_eta[30] = 0.0000000000000000e+00;
+rk_eta[31] = 0.0000000000000000e+00;
+rk_eta[32] = 0.0000000000000000e+00;
+rk_eta[33] = 0.0000000000000000e+00;
+rk_eta[34] = 1.0000000000000000e+00;
+rk_eta[35] = 0.0000000000000000e+00;
+rk_eta[36] = 0.0000000000000000e+00;
+rk_eta[37] = 0.0000000000000000e+00;
+rk_eta[38] = 0.0000000000000000e+00;
+rk_eta[39] = 0.0000000000000000e+00;
+rk_eta[40] = 0.0000000000000000e+00;
+rk_eta[41] = 1.0000000000000000e+00;
+rk_eta[42] = 0.0000000000000000e+00;
+rk_eta[43] = 0.0000000000000000e+00;
+rk_eta[44] = 0.0000000000000000e+00;
+rk_eta[45] = 0.0000000000000000e+00;
+rk_eta[46] = 0.0000000000000000e+00;
+rk_eta[47] = 0.0000000000000000e+00;
+acadoWorkspace.rk_xxx[48] = rk_eta[48];
+acadoWorkspace.rk_xxx[49] = rk_eta[49];
+
+for (run1 = 0; run1 < 1; ++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];
+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];
+acadoWorkspace.rk_xxx[32] = + rk_eta[32];
+acadoWorkspace.rk_xxx[33] = + rk_eta[33];
+acadoWorkspace.rk_xxx[34] = + rk_eta[34];
+acadoWorkspace.rk_xxx[35] = + rk_eta[35];
+acadoWorkspace.rk_xxx[36] = + rk_eta[36];
+acadoWorkspace.rk_xxx[37] = + rk_eta[37];
+acadoWorkspace.rk_xxx[38] = + rk_eta[38];
+acadoWorkspace.rk_xxx[39] = + rk_eta[39];
+acadoWorkspace.rk_xxx[40] = + rk_eta[40];
+acadoWorkspace.rk_xxx[41] = + rk_eta[41];
+acadoWorkspace.rk_xxx[42] = + rk_eta[42];
+acadoWorkspace.rk_xxx[43] = + rk_eta[43];
+acadoWorkspace.rk_xxx[44] = + rk_eta[44];
+acadoWorkspace.rk_xxx[45] = + rk_eta[45];
+acadoWorkspace.rk_xxx[46] = + rk_eta[46];
+acadoWorkspace.rk_xxx[47] = + rk_eta[47];
+acado_rhs_forw( acadoWorkspace.rk_xxx, acadoWorkspace.rk_kkk );
+acadoWorkspace.rk_xxx[0] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[0] + rk_eta[0];
+acadoWorkspace.rk_xxx[1] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[1] + rk_eta[1];
+acadoWorkspace.rk_xxx[2] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[2] + rk_eta[2];
+acadoWorkspace.rk_xxx[3] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[3] + rk_eta[3];
+acadoWorkspace.rk_xxx[4] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[4] + rk_eta[4];
+acadoWorkspace.rk_xxx[5] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[5] + rk_eta[5];
+acadoWorkspace.rk_xxx[6] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[6] + rk_eta[6];
+acadoWorkspace.rk_xxx[7] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[7] + rk_eta[7];
+acadoWorkspace.rk_xxx[8] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[8] + rk_eta[8];
+acadoWorkspace.rk_xxx[9] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[9] + rk_eta[9];
+acadoWorkspace.rk_xxx[10] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[10] + rk_eta[10];
+acadoWorkspace.rk_xxx[11] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[11] + rk_eta[11];
+acadoWorkspace.rk_xxx[12] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[12] + rk_eta[12];
+acadoWorkspace.rk_xxx[13] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[13] + rk_eta[13];
+acadoWorkspace.rk_xxx[14] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[14] + rk_eta[14];
+acadoWorkspace.rk_xxx[15] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[15] + rk_eta[15];
+acadoWorkspace.rk_xxx[16] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[16] + rk_eta[16];
+acadoWorkspace.rk_xxx[17] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[17] + rk_eta[17];
+acadoWorkspace.rk_xxx[18] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[18] + rk_eta[18];
+acadoWorkspace.rk_xxx[19] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[19] + rk_eta[19];
+acadoWorkspace.rk_xxx[20] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[20] + rk_eta[20];
+acadoWorkspace.rk_xxx[21] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[21] + rk_eta[21];
+acadoWorkspace.rk_xxx[22] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[22] + rk_eta[22];
+acadoWorkspace.rk_xxx[23] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[23] + rk_eta[23];
+acadoWorkspace.rk_xxx[24] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[24] + rk_eta[24];
+acadoWorkspace.rk_xxx[25] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[25] + rk_eta[25];
+acadoWorkspace.rk_xxx[26] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[26] + rk_eta[26];
+acadoWorkspace.rk_xxx[27] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[27] + rk_eta[27];
+acadoWorkspace.rk_xxx[28] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[28] + rk_eta[28];
+acadoWorkspace.rk_xxx[29] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[29] + rk_eta[29];
+acadoWorkspace.rk_xxx[30] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[30] + rk_eta[30];
+acadoWorkspace.rk_xxx[31] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[31] + rk_eta[31];
+acadoWorkspace.rk_xxx[32] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[32] + rk_eta[32];
+acadoWorkspace.rk_xxx[33] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[33] + rk_eta[33];
+acadoWorkspace.rk_xxx[34] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[34] + rk_eta[34];
+acadoWorkspace.rk_xxx[35] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[35] + rk_eta[35];
+acadoWorkspace.rk_xxx[36] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[36] + rk_eta[36];
+acadoWorkspace.rk_xxx[37] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[37] + rk_eta[37];
+acadoWorkspace.rk_xxx[38] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[38] + rk_eta[38];
+acadoWorkspace.rk_xxx[39] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[39] + rk_eta[39];
+acadoWorkspace.rk_xxx[40] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[40] + rk_eta[40];
+acadoWorkspace.rk_xxx[41] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[41] + rk_eta[41];
+acadoWorkspace.rk_xxx[42] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[42] + rk_eta[42];
+acadoWorkspace.rk_xxx[43] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[43] + rk_eta[43];
+acadoWorkspace.rk_xxx[44] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[44] + rk_eta[44];
+acadoWorkspace.rk_xxx[45] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[45] + rk_eta[45];
+acadoWorkspace.rk_xxx[46] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[46] + rk_eta[46];
+acadoWorkspace.rk_xxx[47] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[47] + rk_eta[47];
+acado_rhs_forw( acadoWorkspace.rk_xxx, &(acadoWorkspace.rk_kkk[ 48 ]) );
+acadoWorkspace.rk_xxx[0] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[48] + rk_eta[0];
+acadoWorkspace.rk_xxx[1] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[49] + rk_eta[1];
+acadoWorkspace.rk_xxx[2] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[50] + rk_eta[2];
+acadoWorkspace.rk_xxx[3] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[51] + rk_eta[3];
+acadoWorkspace.rk_xxx[4] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[52] + rk_eta[4];
+acadoWorkspace.rk_xxx[5] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[53] + rk_eta[5];
+acadoWorkspace.rk_xxx[6] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[54] + rk_eta[6];
+acadoWorkspace.rk_xxx[7] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[55] + rk_eta[7];
+acadoWorkspace.rk_xxx[8] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[56] + rk_eta[8];
+acadoWorkspace.rk_xxx[9] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[57] + rk_eta[9];
+acadoWorkspace.rk_xxx[10] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[58] + rk_eta[10];
+acadoWorkspace.rk_xxx[11] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[59] + rk_eta[11];
+acadoWorkspace.rk_xxx[12] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[60] + rk_eta[12];
+acadoWorkspace.rk_xxx[13] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[61] + rk_eta[13];
+acadoWorkspace.rk_xxx[14] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[62] + rk_eta[14];
+acadoWorkspace.rk_xxx[15] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[63] + rk_eta[15];
+acadoWorkspace.rk_xxx[16] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[64] + rk_eta[16];
+acadoWorkspace.rk_xxx[17] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[65] + rk_eta[17];
+acadoWorkspace.rk_xxx[18] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[66] + rk_eta[18];
+acadoWorkspace.rk_xxx[19] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[67] + rk_eta[19];
+acadoWorkspace.rk_xxx[20] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[68] + rk_eta[20];
+acadoWorkspace.rk_xxx[21] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[69] + rk_eta[21];
+acadoWorkspace.rk_xxx[22] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[70] + rk_eta[22];
+acadoWorkspace.rk_xxx[23] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[71] + rk_eta[23];
+acadoWorkspace.rk_xxx[24] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[72] + rk_eta[24];
+acadoWorkspace.rk_xxx[25] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[73] + rk_eta[25];
+acadoWorkspace.rk_xxx[26] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[74] + rk_eta[26];
+acadoWorkspace.rk_xxx[27] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[75] + rk_eta[27];
+acadoWorkspace.rk_xxx[28] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[76] + rk_eta[28];
+acadoWorkspace.rk_xxx[29] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[77] + rk_eta[29];
+acadoWorkspace.rk_xxx[30] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[78] + rk_eta[30];
+acadoWorkspace.rk_xxx[31] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[79] + rk_eta[31];
+acadoWorkspace.rk_xxx[32] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[80] + rk_eta[32];
+acadoWorkspace.rk_xxx[33] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[81] + rk_eta[33];
+acadoWorkspace.rk_xxx[34] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[82] + rk_eta[34];
+acadoWorkspace.rk_xxx[35] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[83] + rk_eta[35];
+acadoWorkspace.rk_xxx[36] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[84] + rk_eta[36];
+acadoWorkspace.rk_xxx[37] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[85] + rk_eta[37];
+acadoWorkspace.rk_xxx[38] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[86] + rk_eta[38];
+acadoWorkspace.rk_xxx[39] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[87] + rk_eta[39];
+acadoWorkspace.rk_xxx[40] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[88] + rk_eta[40];
+acadoWorkspace.rk_xxx[41] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[89] + rk_eta[41];
+acadoWorkspace.rk_xxx[42] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[90] + rk_eta[42];
+acadoWorkspace.rk_xxx[43] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[91] + rk_eta[43];
+acadoWorkspace.rk_xxx[44] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[92] + rk_eta[44];
+acadoWorkspace.rk_xxx[45] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[93] + rk_eta[45];
+acadoWorkspace.rk_xxx[46] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[94] + rk_eta[46];
+acadoWorkspace.rk_xxx[47] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[95] + rk_eta[47];
+acado_rhs_forw( acadoWorkspace.rk_xxx, &(acadoWorkspace.rk_kkk[ 96 ]) );
+acadoWorkspace.rk_xxx[0] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[96] + rk_eta[0];
+acadoWorkspace.rk_xxx[1] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[97] + rk_eta[1];
+acadoWorkspace.rk_xxx[2] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[98] + rk_eta[2];
+acadoWorkspace.rk_xxx[3] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[99] + rk_eta[3];
+acadoWorkspace.rk_xxx[4] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[100] + rk_eta[4];
+acadoWorkspace.rk_xxx[5] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[101] + rk_eta[5];
+acadoWorkspace.rk_xxx[6] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[102] + rk_eta[6];
+acadoWorkspace.rk_xxx[7] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[103] + rk_eta[7];
+acadoWorkspace.rk_xxx[8] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[104] + rk_eta[8];
+acadoWorkspace.rk_xxx[9] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[105] + rk_eta[9];
+acadoWorkspace.rk_xxx[10] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[106] + rk_eta[10];
+acadoWorkspace.rk_xxx[11] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[107] + rk_eta[11];
+acadoWorkspace.rk_xxx[12] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[108] + rk_eta[12];
+acadoWorkspace.rk_xxx[13] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[109] + rk_eta[13];
+acadoWorkspace.rk_xxx[14] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[110] + rk_eta[14];
+acadoWorkspace.rk_xxx[15] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[111] + rk_eta[15];
+acadoWorkspace.rk_xxx[16] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[112] + rk_eta[16];
+acadoWorkspace.rk_xxx[17] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[113] + rk_eta[17];
+acadoWorkspace.rk_xxx[18] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[114] + rk_eta[18];
+acadoWorkspace.rk_xxx[19] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[115] + rk_eta[19];
+acadoWorkspace.rk_xxx[20] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[116] + rk_eta[20];
+acadoWorkspace.rk_xxx[21] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[117] + rk_eta[21];
+acadoWorkspace.rk_xxx[22] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[118] + rk_eta[22];
+acadoWorkspace.rk_xxx[23] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[119] + rk_eta[23];
+acadoWorkspace.rk_xxx[24] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[120] + rk_eta[24];
+acadoWorkspace.rk_xxx[25] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[121] + rk_eta[25];
+acadoWorkspace.rk_xxx[26] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[122] + rk_eta[26];
+acadoWorkspace.rk_xxx[27] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[123] + rk_eta[27];
+acadoWorkspace.rk_xxx[28] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[124] + rk_eta[28];
+acadoWorkspace.rk_xxx[29] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[125] + rk_eta[29];
+acadoWorkspace.rk_xxx[30] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[126] + rk_eta[30];
+acadoWorkspace.rk_xxx[31] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[127] + rk_eta[31];
+acadoWorkspace.rk_xxx[32] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[128] + rk_eta[32];
+acadoWorkspace.rk_xxx[33] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[129] + rk_eta[33];
+acadoWorkspace.rk_xxx[34] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[130] + rk_eta[34];
+acadoWorkspace.rk_xxx[35] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[131] + rk_eta[35];
+acadoWorkspace.rk_xxx[36] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[132] + rk_eta[36];
+acadoWorkspace.rk_xxx[37] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[133] + rk_eta[37];
+acadoWorkspace.rk_xxx[38] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[134] + rk_eta[38];
+acadoWorkspace.rk_xxx[39] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[135] + rk_eta[39];
+acadoWorkspace.rk_xxx[40] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[136] + rk_eta[40];
+acadoWorkspace.rk_xxx[41] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[137] + rk_eta[41];
+acadoWorkspace.rk_xxx[42] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[138] + rk_eta[42];
+acadoWorkspace.rk_xxx[43] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[139] + rk_eta[43];
+acadoWorkspace.rk_xxx[44] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[140] + rk_eta[44];
+acadoWorkspace.rk_xxx[45] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[141] + rk_eta[45];
+acadoWorkspace.rk_xxx[46] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[142] + rk_eta[46];
+acadoWorkspace.rk_xxx[47] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[143] + rk_eta[47];
+acado_rhs_forw( acadoWorkspace.rk_xxx, &(acadoWorkspace.rk_kkk[ 144 ]) );
+rk_eta[0] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[0] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[48] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[96] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[144];
+rk_eta[1] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[1] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[49] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[97] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[145];
+rk_eta[2] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[2] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[50] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[98] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[146];
+rk_eta[3] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[3] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[51] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[99] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[147];
+rk_eta[4] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[4] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[52] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[100] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[148];
+rk_eta[5] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[5] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[53] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[101] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[149];
+rk_eta[6] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[6] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[54] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[102] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[150];
+rk_eta[7] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[7] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[55] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[103] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[151];
+rk_eta[8] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[8] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[56] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[104] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[152];
+rk_eta[9] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[9] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[57] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[105] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[153];
+rk_eta[10] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[10] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[58] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[106] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[154];
+rk_eta[11] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[11] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[59] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[107] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[155];
+rk_eta[12] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[12] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[60] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[108] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[156];
+rk_eta[13] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[13] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[61] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[109] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[157];
+rk_eta[14] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[14] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[62] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[110] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[158];
+rk_eta[15] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[15] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[63] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[111] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[159];
+rk_eta[16] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[16] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[64] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[112] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[160];
+rk_eta[17] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[17] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[65] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[113] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[161];
+rk_eta[18] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[18] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[66] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[114] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[162];
+rk_eta[19] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[19] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[67] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[115] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[163];
+rk_eta[20] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[20] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[68] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[116] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[164];
+rk_eta[21] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[21] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[69] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[117] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[165];
+rk_eta[22] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[22] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[70] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[118] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[166];
+rk_eta[23] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[23] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[71] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[119] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[167];
+rk_eta[24] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[24] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[72] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[120] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[168];
+rk_eta[25] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[25] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[73] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[121] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[169];
+rk_eta[26] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[26] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[74] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[122] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[170];
+rk_eta[27] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[27] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[75] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[123] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[171];
+rk_eta[28] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[28] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[76] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[124] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[172];
+rk_eta[29] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[29] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[77] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[125] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[173];
+rk_eta[30] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[30] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[78] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[126] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[174];
+rk_eta[31] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[31] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[79] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[127] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[175];
+rk_eta[32] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[32] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[80] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[128] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[176];
+rk_eta[33] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[33] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[81] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[129] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[177];
+rk_eta[34] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[34] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[82] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[130] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[178];
+rk_eta[35] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[35] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[83] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[131] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[179];
+rk_eta[36] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[36] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[84] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[132] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[180];
+rk_eta[37] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[37] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[85] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[133] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[181];
+rk_eta[38] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[38] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[86] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[134] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[182];
+rk_eta[39] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[39] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[87] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[135] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[183];
+rk_eta[40] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[40] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[88] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[136] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[184];
+rk_eta[41] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[41] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[89] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[137] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[185];
+rk_eta[42] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[42] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[90] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[138] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[186];
+rk_eta[43] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[43] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[91] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[139] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[187];
+rk_eta[44] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[44] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[92] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[140] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[188];
+rk_eta[45] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[45] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[93] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[141] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[189];
+rk_eta[46] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[46] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[94] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[142] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[190];
+rk_eta[47] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[47] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[95] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[143] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[191];
+acadoWorkspace.rk_ttt += 1.0000000000000000e+00;
+}
+error = 0;
+return error;
+}
+

selfdrive/controls/lib/longitudinal_mpc/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(56, 50);
+	
+	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/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      56
+/** Maximum number of constraints. */
+#define QPOASES_NCMAX      50
+/** Maximum number of working set recalculations. */
+#define QPOASES_NWSRMAX    500
+/** 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/pid.py

@@ -12,7 +12,7 @@ def apply_deadzone(error, deadzone):
   return error
 
 class PIController(object):
-  def __init__(self, k_p, k_i, k_f=0., pos_limit=None, neg_limit=None, rate=100, sat_limit=0.8, convert=None):
+  def __init__(self, k_p, k_i, k_f=1., pos_limit=None, neg_limit=None, rate=100, sat_limit=0.8, convert=None):
     self._k_p = k_p # proportional gain
     self._k_i = k_i # integrale gain
     self.k_f = k_f  # feedforward gain
@@ -24,7 +24,6 @@ class PIController(object):
     self.i_unwind_rate = 0.3 / rate
     self.i_rate = 1.0 / rate
     self.sat_limit = sat_limit
-    self.jerk_factor = 0.0
     self.convert = convert
 
     self.reset()
@@ -36,7 +35,7 @@ class PIController(object):
     else:
       kp = interp(self.speed, self._k_p[0], self._k_p[1])
 
-    return (1.0 + self.jerk_factor) * kp
+    return kp
 
   @property
   def k_i(self):
@@ -45,7 +44,7 @@ class PIController(object):
     else:
       ki = interp(self.speed, self._k_i[0], self._k_i[1])
 
-    return (1.0 + self.jerk_factor) * ki
+    return ki
 
   def _check_saturation(self, control, override, error):
     saturated = (control < self.neg_limit) or (control > self.pos_limit)
@@ -62,34 +61,35 @@ class PIController(object):
   def reset(self):
     self.p = 0.0
     self.i = 0.0
+    self.f = 0.0
     self.sat_count = 0.0
     self.saturated = False
     self.control = 0
 
-  def update(self, setpoint, measurement, speed=0.0, check_saturation=True, jerk_factor=0.0, override=False, feedforward=0., deadzone=0.):
+  def update(self, setpoint, measurement, speed=0.0, check_saturation=True, override=False, feedforward=0., deadzone=0., freeze_integrator=False):
     self.speed = speed
-    self.jerk_factor = jerk_factor
 
     error = float(apply_deadzone(setpoint - measurement, deadzone))
     self.p = error * self.k_p
-    f = feedforward * self.k_f
+    self.f = feedforward * self.k_f
 
     if override:
       self.i -= self.i_unwind_rate * float(np.sign(self.i))
     else:
       i = self.i + error * self.k_i * self.i_rate
-      control = self.p + f + i
+      control = self.p + self.f + i
 
       if self.convert is not None:
         control = self.convert(control, speed=self.speed)
 
       # Update when changing i will move the control away from the limits
       # or when i will move towards the sign of the error
-      if (error >= 0 and (control <= self.pos_limit or i < 0.0)) or \
-         (error <= 0 and (control >= self.neg_limit or i > 0.0)):
+      if ((error >= 0 and (control <= self.pos_limit or i < 0.0)) or \
+          (error <= 0 and (control >= self.neg_limit or i > 0.0))) and \
+         not freeze_integrator:
         self.i = i
 
-    control = self.p + f + self.i
+    control = self.p + self.f + self.i
     if self.convert is not None:
       control = self.convert(control, speed=self.speed)
 

selfdrive/controls/lib/planner.py

@@ -1,24 +1,230 @@
 #!/usr/bin/env python
 import zmq
+
+import numpy as np
+import math
+
 from common.realtime import sec_since_boot
+from common.params import Params
+from common.numpy_fast import interp
 import selfdrive.messaging as messaging
+from selfdrive.swaglog import cloudlog
+from selfdrive.config import Conversions as CV
 from selfdrive.services import service_list
 from selfdrive.controls.lib.drive_helpers import create_event, EventTypes as ET
 from selfdrive.controls.lib.pathplanner import PathPlanner
-from selfdrive.controls.lib.adaptivecruise import AdaptiveCruise
-from selfdrive.controls.lib.fcw import ForwardCollisionWarning
+from selfdrive.controls.lib.longitudinal_mpc import libmpc_py
+from selfdrive.controls.lib.speed_smoother import speed_smoother
+from selfdrive.controls.lib.longcontrol import LongCtrlState
+
+_DT = 0.01    # 100Hz
+_DT_MPC = 0.2  # 5Hz
+MAX_SPEED_ERROR = 2.0
+AWARENESS_DECEL = -0.2     # car smoothly decel at .2m/s^2 when user is distracted
+_DEBUG = False
+_LEAD_ACCEL_TAU = 1.5
+
+# 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., 1., .8, .5, .3]
+_A_CRUISE_MAX_V_FOLLOWING = [1.5, 1.5, 1.2, .7, .3]
+_A_CRUISE_MAX_BP = [0.,  5., 10., 20., 40.]
+
+# Lookup table for turns
+_A_TOTAL_MAX_V = [1.5, 1.9, 3.2]
+_A_TOTAL_MAX_BP = [0., 20., 40.]
+
+# max acceleration allowed in acc, which happens in restart
+A_ACC_MAX = max(_A_CRUISE_MAX_V_FOLLOWING)
+
+
+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
+  this should avoid accelerating when losing the target in turns
+  """
+  deg_to_rad = np.pi / 180.  # from can reading to rad
+
+  a_total_max = interp(v_ego, _A_TOTAL_MAX_BP, _A_TOTAL_MAX_V)
+  a_y = v_ego**2 * angle_steers * deg_to_rad / (CP.sR * CP.l)
+  a_x_allowed = math.sqrt(max(a_total_max**2 - a_y**2, 0.))
+
+  a_target[1] = min(a_target[1], a_x_allowed)
+  return a_target
+
+
+class FCWChecker(object):
+  def __init__(self):
+    self.fcw_count = 0
+    self.last_fcw_a = 0.0
+    self.v_lead_max = 0.0
+    self.lead_seen_t = 0.0
+    self.last_fcw_time = 0.0
+
+  def reset_lead(self, cur_time):
+    self.v_lead_max = 0.0
+    self.lead_seen_t = cur_time
+
+  def update(self, mpc_solution, cur_time, v_ego, v_lead, y_lead, vlat_lead, fcw_lead, blinkers):
+    min_a_mpc = min(list(mpc_solution[0].a_ego)[1:])
+    self.v_lead_max = max(self.v_lead_max, v_lead)
+
+    if (fcw_lead > 0.99
+        and v_ego > 5.0
+        and min_a_mpc < -4.0
+        and self.v_lead_max > 2.5
+        and v_ego > v_lead
+        and self.lead_seen_t < cur_time - 2.0
+        and abs(y_lead) < 1.0
+        and abs(vlat_lead) < 0.3
+        and not blinkers):
+      self.fcw_count += 1
+      if self.fcw_count > 10 and self.last_fcw_time + 5.0 < cur_time:
+        self.last_fcw_time = cur_time
+        self.last_fcw_a = min_a_mpc
+        return True
+    else:
+      self.fcw_count = 0
+
+    return False
+
+
+class LongitudinalMpc(object):
+  def __init__(self, mpc_id, live_longitudinal_mpc):
+    self.live_longitudinal_mpc = live_longitudinal_mpc
+    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
+
+    self.last_cloudlog_t = 0.0
+
+  def send_mpc_solution(self, qp_iterations, calculation_time):
+    qp_iterations = max(0, qp_iterations)
+    dat = messaging.new_message()
+    dat.init('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.aLead = list(self.mpc_solution[0].a_l)
+    dat.liveLongitudinalMpc.aLeadTau = self.l
+    dat.liveLongitudinalMpc.qpIterations = qp_iterations
+    dat.liveLongitudinalMpc.mpcId = self.mpc_id
+    dat.liveLongitudinalMpc.calculationTime = calculation_time
+    self.live_longitudinal_mpc.send(dat.to_bytes())
+
+  def setup_mpc(self):
+    ffi, self.libmpc = libmpc_py.get_libmpc(self.mpc_id)
+    self.libmpc.init()
+
+    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.l = _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_cruise_setpoint):
+    # 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
+
+      # Learn if constant acceleration
+      if abs(a_lead) < 0.5:
+        self.l = _LEAD_ACCEL_TAU
+      else:
+        self.l *= 0.9
+
+      l = max(self.l, -a_lead / (v_lead + 0.01))
+      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, l)
+        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
+      self.cur_state[0].a_l = a_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 = CS.vEgo + 10.0
+      self.cur_state[0].a_l = 0.0
+      l = _LEAD_ACCEL_TAU
+
+    # Calculate mpc
+    t = sec_since_boot()
+    n_its = self.libmpc.run_mpc(self.cur_state, self.mpc_solution, l)
+    duration = int((sec_since_boot() - t) * 1e9)
+    self.send_mpc_solution(n_its, duration)
 
-_DT = 0.01   # 100Hz
+    # 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]
+
+    # Reset if NaN or goes through lead car
+    dls = np.array(list(self.mpc_solution[0].x_l)[1:]) - np.array(list(self.mpc_solution[0].x_ego)[1:])
+    crashing = min(dls) < -50.0
+    nans = np.any(np.isnan(list(self.mpc_solution[0].v_ego)))
+    backwards = min(list(self.mpc_solution[0].v_ego)[1:]) < -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.mpc_id, backwards, crashing, nans))
+
+      self.libmpc.init()
+      self.cur_state[0].v_ego = CS.vEgo
+      self.cur_state[0].a_ego = 0.0
+      self.prev_lead_status = False
 
 
 class Planner(object):
-  def __init__(self, CP):
+  def __init__(self, CP, fcw_enabled):
     context = zmq.Context()
     self.CP = CP
     self.live20 = messaging.sub_sock(context, service_list['live20'].port)
     self.model = messaging.sub_sock(context, service_list['model'].port)
-
     self.plan = messaging.pub_sock(context, service_list['plan'].port)
+    self.live_longitudinal_mpc = messaging.pub_sock(context, service_list['liveLongitudinalMpc'].port)
 
     self.last_md_ts = 0
     self.last_l20_ts = 0
@@ -29,36 +235,141 @@ class Planner(object):
     self.radar_errors = []
 
     self.PP = PathPlanner()
-    self.AC = AdaptiveCruise()
-    self.FCW = ForwardCollisionWarning(_DT)
+    self.mpc1 = LongitudinalMpc(1, self.live_longitudinal_mpc)
+    self.mpc2 = LongitudinalMpc(2, self.live_longitudinal_mpc)
+
+    self.v_acc_start = 0.0
+    self.a_acc_start = 0.0
+    self.acc_start_time = sec_since_boot()
+    self.v_acc = 0.0
+    self.v_acc_sol = 0.0
+    self.v_acc_future = 0.0
+    self.a_acc = 0.0
+    self.a_acc_sol = 0.0
+    self.v_cruise = 0.0
+    self.a_cruise = 0.0
+
+    self.lead_1 = None
+    self.lead_2 = None
+
+    self.longitudinalPlanSource = 'cruise'
+    self.fcw = False
+    self.fcw_checker = FCWChecker()
+    self.fcw_enabled = fcw_enabled
+
+  def choose_solution(self, v_cruise_setpoint):
+    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)
+
+    if _DEBUG:
+      print "D_SOL", solutions, slowest, self.v_acc_sol, self.a_acc_sol
+      print "D_V", self.mpc1.v_mpc, self.mpc2.v_mpc, self.v_cruise
+      print "D_A", self.mpc1.a_mpc, self.mpc2.a_mpc, self.a_cruise
+
+    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])
 
   # this runs whenever we get a packet that can change the plan
-  def update(self, CS, LoC):
+  def update(self, CS, LoC, v_cruise_kph, user_distracted):
     cur_time = sec_since_boot()
+    v_cruise_setpoint = v_cruise_kph * CV.KPH_TO_MS
 
     md = messaging.recv_sock(self.model)
     if md is not None:
       self.last_md_ts = md.logMonoTime
       self.last_model = cur_time
       self.model_dead = False
-    if cur_time - self.last_model > 0.5:
-      self.model_dead = True
 
-    l20 = messaging.recv_sock(self.live20)
+      self.PP.update(CS.vEgo, md)
+
+    l20 = messaging.recv_sock(self.live20) if md is None else None
     if l20 is not None:
       self.last_l20_ts = l20.logMonoTime
       self.last_l20 = cur_time
       self.radar_dead = False
       self.radar_errors = list(l20.live20.radarErrors)
-    if cur_time - self.last_l20 > 0.5:
-      self.radar_dead = True
 
-    self.PP.update(CS.vEgo, md)
+      self.v_acc_start = self.v_acc_sol
+      self.a_acc_start = self.a_acc_sol
+      self.acc_start_time = cur_time
 
-    # LoC.v_pid -> CS.vEgo
-    # TODO: is this change okay?
-    self.AC.update(CS.vEgo, CS.steeringAngle, LoC.v_pid, self.CP, l20)
+      self.lead_1 = l20.live20.leadOne
+      self.lead_2 = l20.live20.leadTwo
 
+      enabled = (LoC.long_control_state == LongCtrlState.pid) or (LoC.long_control_state == LongCtrlState.stopping)
+      following = self.lead_1.status and self.lead_1.dRel < 45.0 and self.lead_1.vLeadK > CS.vEgo and self.lead_1.aLeadK > 0.0
+
+      # Calculate speed for normal cruise control
+      if enabled:
+
+        accel_limits = map(float, calc_cruise_accel_limits(CS.vEgo, following))
+        # TODO: make a separate lookup for jerk tuning
+        jerk_limits = [min(-0.1, accel_limits[0]), max(0.1, accel_limits[1])]
+        accel_limits = limit_accel_in_turns(CS.vEgo, CS.steeringAngle, accel_limits, self.CP)
+        if user_distracted:
+          # if user is not responsive to awareness alerts, then start a smooth deceleration
+          accel_limits[1] = min(accel_limits[1], AWARENESS_DECEL)
+          accel_limits[0] = min(accel_limits[0], accel_limits[1])
+
+        self.v_cruise, self.a_cruise = speed_smoother(self.v_acc_start, self.a_acc_start,
+                                                      v_cruise_setpoint,
+                                                      accel_limits[1], accel_limits[0],
+                                                      jerk_limits[1],
+                                                      jerk_limits[0],
+                                                      _DT_MPC)
+      else:
+        starting = LoC.long_control_state == LongCtrlState.starting
+        self.v_cruise = CS.vEgo
+        self.a_cruise = self.CP.startAccel if starting else CS.aEgo
+        self.v_acc_start = CS.vEgo
+        self.a_acc_start = self.CP.startAccel if starting else CS.aEgo
+        self.v_acc = CS.vEgo
+        self.a_acc = self.CP.startAccel if starting else CS.aEgo
+        self.v_acc_sol = CS.vEgo
+        self.a_acc_sol = self.CP.startAccel if starting else CS.aEgo
+
+      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(CS, self.lead_1, v_cruise_setpoint)
+      self.mpc2.update(CS, self.lead_2, v_cruise_setpoint)
+
+      self.choose_solution(v_cruise_setpoint)
+
+      # determine fcw
+      if self.mpc1.new_lead:
+        self.fcw_checker.reset_lead(cur_time)
+
+      blinkers = CS.leftBlinker or CS.rightBlinker
+      self.fcw = self.fcw_checker.update(self.mpc1.mpc_solution, cur_time, CS.vEgo,
+                                         self.lead_1.vLead, self.lead_1.yRel, self.lead_1.vLat,
+                                         self.lead_1.fcw, blinkers) \
+                 and not CS.brakePressed
+      if self.fcw:
+        cloudlog.info("FCW triggered")
+
+    if cur_time - self.last_model > 0.5:
+      self.model_dead = True
+
+    if cur_time - self.last_l20 > 0.5:
+      self.radar_dead = True
     # **** send the plan ****
     plan_send = messaging.new_message()
     plan_send.init('plan')
@@ -71,8 +382,12 @@ class Planner(object):
     if 'fault' in self.radar_errors:
       events.append(create_event('radarFault', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
 
-    plan_send.plan.events = events
+    # Interpolation of trajectory
+    dt = min(cur_time - self.acc_start_time, _DT_MPC + _DT) + _DT  # no greater than dt mpc + dt, to prevent too high extraps
+    self.a_acc_sol = self.a_acc_start + (dt / _DT_MPC) * (self.a_acc - self.a_acc_start)
+    self.v_acc_sol = self.v_acc_start + dt * (self.a_acc_sol + self.a_acc_start) / 2.0
 
+    plan_send.plan.events = events
     plan_send.plan.mdMonoTime = self.last_md_ts
     plan_send.plan.l20MonoTime = self.last_l20_ts
 
@@ -83,15 +398,17 @@ class Planner(object):
 
     # longitudal plan
     plan_send.plan.longitudinalValid = not self.radar_dead
-    plan_send.plan.vTarget = float(self.AC.v_target_lead)
-    plan_send.plan.aTargetMin = float(self.AC.a_target[0])
-    plan_send.plan.aTargetMax = float(self.AC.a_target[1])
-    plan_send.plan.jerkFactor = float(self.AC.jerk_factor)
-    plan_send.plan.hasLead = self.AC.has_lead
-
-    # compute risk of collision events: fcw
-    self.FCW.process(CS, self.AC)
-    plan_send.plan.fcw = bool(self.FCW.active)
+    plan_send.plan.vCruise = self.v_cruise
+    plan_send.plan.aCruise = self.a_cruise
+    plan_send.plan.vTarget = self.v_acc_sol
+    plan_send.plan.aTarget = self.a_acc_sol
+    plan_send.plan.vTargetFuture = self.v_acc_future
+    plan_send.plan.hasLead = self.mpc1.prev_lead_status
+    plan_send.plan.longitudinalPlanSource = self.longitudinalPlanSource
+
+    # Send out fcw
+    fcw = self.fcw and (self.fcw_enabled or LoC.long_control_state != LongCtrlState.off)
+    plan_send.plan.fcw = fcw
 
     self.plan.send(plan_send.to_bytes())
     return plan_send

selfdrive/controls/lib/speed_smoother.py

@@ -0,0 +1,88 @@
+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
+
+  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
+    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
+  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/radard.py

@@ -79,8 +79,9 @@ def radard_thread(gctx=None):
   tsv = 1./rate
   v_len = 20         # how many speed data points to remember for t alignment with rdr data
 
-  enabled = 0
+  active = 0
   steer_angle = 0.
+  steer_override = False
 
   tracks = defaultdict(dict)
 
@@ -104,9 +105,10 @@ def radard_thread(gctx=None):
     # receive the live100s
     l100 = messaging.recv_sock(live100)
     if l100 is not None:
-      enabled = l100.live100.enabled
+      active = l100.live100.active
       v_ego = l100.live100.vEgo
       steer_angle = l100.live100.angleSteers
+      steer_override = l100.live100.steerOverride
 
       v_ego_array = np.append(v_ego_array, [[v_ego], [float(rk.frame)/rate]], 1)
       v_ego_array = v_ego_array[:, 1:]
@@ -137,7 +139,7 @@ def radard_thread(gctx=None):
         del ar_pts[VISION_POINT]
 
     # *** compute the likely path_y ***
-    if enabled:    # use path from model path_poly
+    if active and not steer_override:    # use path from model path_poly
       path_y = np.polyval(PP.d_poly, path_x)
     else:          # use path from steer, set angle_offset to 0 since calibration does not exactly report the physical offset
       path_y = calc_lookahead_offset(v_ego, steer_angle, path_x, VM, angle_offset=0)[0]

selfdrive/manager.py

@@ -581,6 +581,8 @@ def main():
     params.put("IsMetric", "0")
   if params.get("IsRearViewMirror") is None:
     params.put("IsRearViewMirror", "1")
+  if params.get("IsFcwEnabled") is None:
+    params.put("IsFcwEnabled", "1")
 
   params.put("Passive", "1" if os.getenv("PASSIVE") else "0")
 

selfdrive/radar/toyota/interface.py

@@ -8,16 +8,18 @@ import zmq
 from selfdrive.services import service_list
 import selfdrive.messaging as messaging
 
+
+RADAR_MSGS = range(0x210, 0x220)
+
 def _create_radard_can_parser():
   dbc_f = 'toyota_prius_2017_adas.dbc'
-  radar_messages = range(0x210, 0x220)
-  msg_n = len(radar_messages)
-  msg_last = radar_messages[-1]
+  msg_n = len(RADAR_MSGS)
+  msg_last = RADAR_MSGS[-1]
   signals = zip(['LONG_DIST'] * msg_n + ['NEW_TRACK'] * msg_n + ['LAT_DIST'] * msg_n +
                 ['REL_SPEED'] * msg_n + ['VALID'] * msg_n,
-                radar_messages * 5,
+                RADAR_MSGS * 5,
                 [255] * msg_n + [1] * msg_n + [0] * msg_n + [0] * msg_n + [0] * msg_n)
-  checks = zip(radar_messages, [20]*msg_n)
+  checks = zip(RADAR_MSGS, [20]*msg_n)
 
   return CANParser(os.path.splitext(dbc_f)[0], signals, checks, 1)
 
@@ -25,6 +27,7 @@ class RadarInterface(object):
   def __init__(self):
     # radar
     self.pts = {}
+    self.ptsValid = {key: False for key in RADAR_MSGS}
     self.track_id = 0
 
     self.delay = 0.0  # Delay of radar
@@ -55,7 +58,17 @@ class RadarInterface(object):
     #print "NEW TRACKS"
     for ii in updated_messages:
       cpt = self.rcp.vl[ii]
-      if cpt['LONG_DIST'] < 255 and cpt['VALID']:
+
+      # a point needs one valid measurement before being considered
+      #if cpt['NEW_TRACK'] or cpt['LONG_DIST'] >= 255:
+      #  self.ptsValid[ii] = False    # reset validity
+      # TODO: find better way to eliminate both false positive and false negative
+      if cpt['VALID'] and cpt['LONG_DIST'] < 255:
+        self.ptsValid[ii] = True
+      else:
+        self.ptsValid[ii] = False
+
+      if self.ptsValid[ii]:
         #print "%5s %5s %5s" % (round(cpt['LONG_DIST'], 1), round(cpt['LAT_DIST'], 1), round(cpt['REL_SPEED'], 1))
         if ii not in self.pts or cpt['NEW_TRACK']:
           self.pts[ii] = car.RadarState.RadarPoint.new_message()

selfdrive/service_list.yaml

@@ -45,6 +45,7 @@ gpsLocationExternal: [8032, true]
 ubloxGnss: [8033, true]
 clocks: [8034, true]
 liveMpc: [8035, true]
+liveLongitudinalMpc: [8036, true]
 
 testModel: [8040, false]
 

selfdrive/test/plant/maneuver.py

@@ -64,7 +64,7 @@ class Maneuver(object):
           v_target_lead=last_live100.vTargetLead, pid_speed=last_live100.vPid,
           cruise_speed=last_live100.vCruise,
           jerk_factor=last_live100.jerkFactor,
-          a_target_min=last_live100.aTargetMin, a_target_max=last_live100.aTargetMax)
+          a_target=last_live100.aTarget)
     
     print "maneuver end"
 

selfdrive/test/plant/maneuverplots.py

@@ -29,8 +29,7 @@ class ManeuverPlot(object):
     self.cruise_speed_array = []
     self.jerk_factor_array = []
 
-    self.a_target_min_array = []
-    self.a_target_max_array = []
+    self.a_target_array = []
 
     self.v_target_array = []
 
@@ -38,8 +37,7 @@ class ManeuverPlot(object):
     
   def add_data(self, time, gas, brake, steer_torque, distance, speed, 
     acceleration, up_accel_cmd, ui_accel_cmd, d_rel, v_rel, v_lead, 
-    v_target_lead, pid_speed, cruise_speed, jerk_factor, a_target_min, 
-    a_target_max):
+    v_target_lead, pid_speed, cruise_speed, jerk_factor, a_target):
     self.time_array.append(time)
     self.gas_array.append(gas)
     self.brake_array.append(brake)
@@ -56,8 +54,7 @@ class ManeuverPlot(object):
     self.pid_speed_array.append(pid_speed)
     self.cruise_speed_array.append(cruise_speed)
     self.jerk_factor_array.append(jerk_factor)
-    self.a_target_min_array.append(a_target_min)
-    self.a_target_max_array.append(a_target_max)
+    self.a_target_array.append(a_target)
 
 
   def write_plot(self, path, maneuver_name):
@@ -90,12 +87,11 @@ class ManeuverPlot(object):
     plt.figure(plt_num)
     plt.plot(
       np.array(self.time_array), np.array(self.acceleration_array), 'g',
-      np.array(self.time_array), np.array(self.a_target_min_array), 'k--',
-      np.array(self.time_array), np.array(self.a_target_max_array), 'k--',
+      np.array(self.time_array), np.array(self.a_target_array), 'k--',
     )
     plt.xlabel('Time [s]')
     plt.ylabel('Acceleration [m/s^2]')
-    plt.legend(['accel', 'max', 'min'], loc=0)
+    plt.legend(['ego-plant', 'target'], loc=0)
     plt.grid()
     pylab.savefig("/".join([path, maneuver_name, 'acceleration.svg']), dpi=1000)
 

selfdrive/test/plant/plant.py

@@ -253,7 +253,8 @@ class Plant(object):
     Plant.logcan.send(can_list_to_can_capnp(can_msgs).to_bytes())
 
     # ******** publish a fake model going straight and fake calibration ********
-    if publish_model:
+    # note that this is worst case for MPC, since model will delay long mpc by one time step
+    if publish_model and self.rk.frame % 5 == 0:
       md = messaging.new_message()
       cal = messaging.new_message()
       md.init('model')

selfdrive/test/tests/plant/test_longitudinal.py

@@ -156,6 +156,18 @@ maneuvers = [
                             (CB.RES_ACCEL, 1.4), (0.0, 1.5),
                             (CB.RES_ACCEL, 1.6), (0.0, 1.7)]
   ),
+  Maneuver(
+    "stop and go with 1.5m/s2 lead accel and 3.3m/s^2 lead decel, with full stops",
+    duration=45.,
+    initial_speed=0.,
+    lead_relevancy=True,
+    initial_distance_lead=20.,
+    speed_lead_values=[10., 0., 0., 10., 0., 0.] ,
+    speed_lead_breakpoints=[10., 13., 26., 33., 36., 45.],
+    cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3),
+                            (CB.RES_ACCEL, 1.4), (0.0, 1.5),
+                            (CB.RES_ACCEL, 1.6), (0.0, 1.7)]
+  ),
   Maneuver(
     "accelerate from 20 while lead vehicle decelerates from 40 to 20 at 1m/s2",
     duration=30.,