openpilot v0.4.0.1 release

README.md

@@ -3,7 +3,7 @@ Welcome to openpilot
 
 [openpilot](http://github.com/commaai/openpilot) is an open source driving agent.
 
-Currently it performs the functions of Adaptive Cruise Control (ACC) and Lane Keeping Assist System (LKAS) for Hondas and Acuras. It's about on par with Tesla Autopilot at launch, and better than [all other manufacturers](http://www.thedrive.com/tech/5707/the-war-for-autonomous-driving-part-iii-us-vs-germany-vs-japan).
+Currently it performs the functions of Adaptive Cruise Control (ACC) and Lane Keeping Assist System (LKAS) for Hondas, Acuras and Toyotas. It's about on par with Tesla Autopilot at launch, and better than [all other manufacturers](http://www.thedrive.com/tech/5707/the-war-for-autonomous-driving-part-iii-us-vs-germany-vs-japan).
 
 The openpilot codebase has been written to be concise and enable rapid prototyping. We look forward to your contributions - improving real vehicle automation has never been easier.
 
@@ -29,13 +29,18 @@ Supported Cars
 - Honda CR-V Touring 2015-2016
   - Can only be enabled above 25 mph
 
-- Toyota RAV-4 2016+ with TSS-P (alpha!)
+- Toyota RAV-4 2016+ non-hybrid with TSS-P
   - By default it uses stock Toyota ACC for longitudinal control
-  - openpilot longitudinal control available after unplugging the [Driving Support ECU](https://community.comma.ai/wiki/index.php/Toyota#Prius_.28for_openpilot.29) and can be enabled above 20 mph
+  - openpilot longitudinal control available after unplugging the [Driving Support ECU](https://community.comma.ai/wiki/index.php/Toyota#Rav4_.28for_openpilot.29) 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](https://community.comma.ai/wiki/index.php/Toyota#Rav4_.28for_openpilot.29)
+  - openpilot longitudinal control available after unplugging the [Driving Support ECU](https://community.comma.ai/wiki/index.php/Toyota#Prius_.28for_openpilot.29)
+  - Lateral control needs improvements
+
+- Toyota RAV-4 2017 hybrid (alpha!)
+  - By default it uses stock Toyota ACC for longitudinal control
+  - openpilot longitudinal control available after unplugging the [Driving Support ECU](https://community.comma.ai/wiki/index.php/Toyota#Rav4_.28for_openpilot.29) and can do stop and go
 
 In Progress Cars
 ------
@@ -50,6 +55,12 @@ Community WIP Cars
 
 - [Classic Tesla Model S (pre-AP)](https://github.com/commaai/openpilot/pull/145)
 
+- [Honda Odyssey 2018 with Honda Sensing](https://github.com/commaai/openpilot/pull/155)
+
+- [Honda Pilot 2017 with Honda Sensing](https://github.com/commaai/openpilot/pull/161)
+
+- [Acura RDX 2018 with AcuraWatch Plus](https://github.com/commaai/openpilot/pull/162)
+
 Directory structure
 ------
 

RELEASES.md

@@ -1,3 +1,12 @@
+Version 0.4.0.1 (2017-12-21)
+==========================
+ * New UI to match chffrplus
+ * Improved lateral control tuning to fix oscillations on Civic
+ * Add alpha support for 2017 Toyota Rav4 Hybrid
+ * Reduced CPU usage
+ * Removed unnecessary utilization of fan at max speed
+ * Minor bug fixes
+
 Version 0.3.9 (2017-11-21)
 ==========================
  * Add alpha support for 2017 Toyota Prius

cereal/car.capnp

@@ -12,12 +12,13 @@ $Java.outerClassname("Car");
 struct CarEvent @0x9b1657f34caf3ad3 {
   name @0 :EventName;
   enable @1 :Bool;
-  preEnable @7 :Bool;
   noEntry @2 :Bool;
   warning @3 :Bool;
   userDisable @4 :Bool;
   softDisable @5 :Bool;
   immediateDisable @6 :Bool;
+  preEnable @7 :Bool;
+  permanent @8 :Bool;
 
   enum EventName @0xbaa8c5d505f727de {
     # TODO: copy from error list
@@ -64,9 +65,10 @@ struct CarState {
   events @13 :List(CarEvent);
 
   # car speed
-  vEgo @1 :Float32;       # best estimate of speed
-  aEgo @16 :Float32;       # best estimate of acceleration
-  vEgoRaw @17 :Float32;       # unfiltered speed
+  vEgo @1 :Float32;         # best estimate of speed
+  aEgo @16 :Float32;        # best estimate of acceleration
+  vEgoRaw @17 :Float32;     # unfiltered speed from CAN sensors
+  yawRate @22 :Float32;     # best estimate of yaw rate
   standstill @18 :Bool;
   wheelSpeeds @2 :WheelSpeeds;
 
@@ -309,4 +311,5 @@ struct CarParams {
   directAccelControl @31 :Bool; # Does the car have direct accel control or just gas/brake
   stoppingControl @34 :Bool; # Does the car allows full control even at lows speeds when stopping
   startAccel @35 :Float32; # Required acceleraton to overcome creep braking
+  steerRateCost @40 :Float32; # Lateral MPC cost on steering rate
 }

cereal/log.capnp

@@ -28,6 +28,7 @@ struct InitData {
   version @4 :Text;
   gitCommit @10 :Text;
   gitBranch @11 :Text;
+  gitRemote @13 :Text;
 
   androidBuildInfo @5 :AndroidBuildInfo;
   androidSensors @6 :List(AndroidSensor);
@@ -154,6 +155,8 @@ struct SensorEventData {
     pressure @9 :SensorVec;
     magneticUncalibrated @11 :SensorVec;
     gyroUncalibrated @12 :SensorVec;
+    proximity @13: Float32;
+    light @14: Float32;
   }
   source @8 :SensorSource;
 
@@ -242,8 +245,11 @@ struct ThermalData {
   freeSpace @7 :Float32;
   batteryPercent @8 :Int16;
   batteryStatus @9 :Text;
+  usbOnline @12 :Bool;
 
   fanSpeed @10 :UInt16;
+  started @11 :Bool;
+  startedTs @13 :UInt64;
 }
 
 struct HealthData {
@@ -349,6 +355,7 @@ struct Live100Data {
   jerkFactor @12 :Float32;
   angleSteers @13 :Float32;     # Steering angle in degrees.
   angleSteersDes @29 :Float32;
+  curvature @37 :Float32;       # path curvature from vehicle model
   hudLeadDEPRECATED @14 :Int32;
   cumLagMs @15 :Float32;
 
@@ -361,6 +368,8 @@ struct Live100Data {
   rearViewCam @23 :Bool;
   alertText1 @24 :Text;
   alertText2 @25 :Text;
+  alertStatus @38 :AlertStatus;
+  alertSize @39 :AlertSize;
   awarenessStatus @26 :Float32;
 
   angleOffset @27 :Float32;
@@ -378,6 +387,20 @@ struct Live100Data {
     stopping @2;
     starting @3;
   }
+
+  enum AlertStatus {
+    normal @0;       # low priority alert for user's convenience
+    userPrompt @1;   # mid piority alert that might require user intervention
+    critical @2;     # high priority alert that needs immediate user intervention
+  }
+
+  enum AlertSize {
+    none @0;    # don't display the alert
+    small @1;   # small box
+    mid @2;     # mid screen
+    full @3;    # full screen
+  }
+
 }
 
 struct LiveEventData {
@@ -600,6 +623,23 @@ struct NavUpdate {
   }
 }
 
+struct NavStatus {
+  isNavigating @0 :Bool;
+  currentAddress @1 :Address;
+
+  struct Address {
+    title @0 :Text;
+    lat @1 :Float64;
+    lng @2 :Float64;
+    house @3 :Text;
+    address @4 :Text;
+    street @5 :Text;
+    city @6 :Text;
+    state @7 :Text;
+    country @8 :Text;
+  }
+}
+
 struct CellInfo {
   timestamp @0 :UInt64;
   repr @1 :Text; # android toString() for now
@@ -1300,5 +1340,6 @@ struct Event {
     clocks @35 :Clocks;
     liveMpc @36 :LiveMpcData;
     liveLongitudinalMpc @37 :LiveLongitudinalMpcData;
+    navStatus @38 :NavStatus;
   }
 }

common/basedir.py

@@ -1,4 +1,4 @@
 import os
-BASEDIR = os.path.join(os.path.dirname(os.path.realpath(__file__)), "../")
+BASEDIR = os.path.abspath(os.path.join(os.path.dirname(os.path.realpath(__file__)), "../"))
 
 

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 RAV4 2017 HYBRID": {
+    36L: 8, 37L: 8, 170L: 8, 180L: 8, 296L: 8, 426L: 6, 452L: 8, 466L: 8, 467L: 8, 550L: 8, 552L: 4, 560L: 7, 581L: 5, 608L: 8, 610L: 5, 643L: 7, 713L: 8, 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, 921L: 8, 933L: 8, 944L: 8, 945L: 8, 950L: 8, 951L: 8, 953L: 3, 955L: 8, 956L: 8, 979L: 2, 998L: 5, 999L: 7, 1000L: 8, 1001L: 8, 1005L: 2, 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, 1184L: 8, 1185L: 8, 1186L: 8, 1190L: 8, 1191L: 8, 1192L: 8, 1196L: 8, 1197L: 8, 1198L: 8, 1199L: 8, 1212L: 8, 1227L: 8, 1232L: 8, 1235L: 8, 1264L: 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, 1596L: 8, 1597L: 8, 1600L: 8, 1664L: 8, 1728L: 8, 1745L: 8, 1779L: 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
   },

common/kalman/ekf.py

@@ -35,7 +35,7 @@ class SensorReading:
 # A generic sensor class that does no pre-processing of data
 class SimpleSensor:
   # obs_model can be
-  #   a full obesrvation model matrix, or
+  #   a full observation model matrix, or
   #   an integer or tuple of indices into ekf.state, indicating which variables are being directly observed
   # covar can be
   #   a full covariance matrix
@@ -129,7 +129,7 @@ class EKF:
       print "covar:\n",self.covar
 
   def update_scalar(self, reading):
-    # like update but knowing that measurment is a scalar
+    # like update but knowing that measurement is a scalar
     # this avoids matrix inversions and speeds up (surprisingly) drived.py a lot
 
     # innovation = reading.data - np.matmul(reading.obs_model, self.state)
@@ -188,7 +188,7 @@ class EKF:
 
     Current implementations calculate A and J as functions of state. Control input
       can be added trivially by adding a control parameter to predict() and calc_tranfer_update(),
-      and using it during calcualtion of A and J
+      and using it during calculation of A and J
     """
 
 

common/params.py

@@ -52,12 +52,14 @@ keys = {
   "Version": TxType.PERSISTANT,
   "GitCommit": TxType.PERSISTANT,
   "GitBranch": TxType.PERSISTANT,
+  "GitRemote": TxType.PERSISTANT,
 # written: baseui
 # read:    ui, controls
   "IsMetric": TxType.PERSISTANT,
   "IsRearViewMirror": TxType.PERSISTANT,
   "IsFcwEnabled": TxType.PERSISTANT,
   "HasAcceptedTerms": TxType.PERSISTANT,
+  "IsUploadVideoOverCellularEnabled": TxType.PERSISTANT,
 # written: visiond
 # read:    visiond, controlsd
   "CalibrationParams": TxType.PERSISTANT,
@@ -69,6 +71,7 @@ keys = {
   "CarParams": TxType.CLEAR_ON_CAR_START,
 
   "Passive": TxType.PERSISTANT,
+  "DoUninstall": TxType.CLEAR_ON_MANAGER_START,
 }
 
 def fsync_dir(path):

common/profiler.py

@@ -1,32 +1,47 @@
-from common.realtime import sec_since_boot
+import time
 
 class Profiler(object):
   def __init__(self, enabled=False):
     self.enabled = enabled
-    self.cp = []
-    self.start_time = sec_since_boot()
+    self.cp = {}
+    self.cp_ignored = []
+    self.iter = 0
+    self.start_time = time.clock()
     self.last_time = self.start_time
-
-  def checkpoint(self, name):
-    if not self.enabled:
-      return
-    tt = sec_since_boot()
-    self.cp.append((name, tt - self.last_time))
-    self.last_time = tt
+    self.tot = 0.
 
   def reset(self, enabled=False):
     self.enabled = enabled
-    self.cp = []
-    self.start_time = sec_since_boot()
+    self.cp = {}
+    self.cp_ignored = []
+    self.iter = 0
+    self.start_time = time.clock()
     self.last_time = self.start_time
 
+  def checkpoint(self, name, ignore=False):
+    # ignore flag needed when benchmarking threads with ratekeeper
+    if not self.enabled:
+      return
+    tt = time.clock()
+    if name not in self.cp:
+      self.cp[name] = 0.
+      if ignore:
+        self.cp_ignored.append(name)
+    self.cp[name] += tt - self.last_time
+    if not ignore:
+      self.tot += tt - self.last_time
+    self.last_time = tt
+
   def display(self):
     if not self.enabled:
       return
+    self.iter += 1
     print "******* Profiling *******"
-    tot = 0.0
-    for n, ms in self.cp:
-      print "%30s: %7.2f" % (n, ms*1000.0)
-      tot += ms
-    print "    TOTAL: %7.2f" % (tot*1000.0)
+    for n in self.cp:
+      ms = self.cp[n]
+      if n in self.cp_ignored:
+        print "%30s: %7.2f   perc: %1.0f" % (n, ms*1000.0, ms/self.tot*100), "  IGNORED"
+      else:
+        print "%30s: %7.2f   perc: %1.0f" % (n, ms*1000.0, ms/self.tot*100)
+    print "Iter clock: %2.6f   TOTAL: %2.2f" % (self.tot/self.iter, self.tot)
 

launch_openpilot.sh

@@ -0,0 +1,39 @@
+#!/usr/bin/bash
+
+function launch {
+  # apply update
+  if [ "$(git rev-parse HEAD)" != "$(git rev-parse @{u})" ]; then
+     git reset --hard @{u} &&
+     git clean -xdf &&
+     exec "${BASH_SOURCE[0]}"
+  fi
+
+  # check if NEOS update is required
+  while [ "$(cat /VERSION)" -lt 4 ] && [ ! -e /data/media/0/noupdate ]; do
+    curl -o /tmp/updater https://neos.comma.ai/updater && chmod +x /tmp/updater && /tmp/updater
+    sleep 10
+  done
+
+  # no cpu rationing for now
+  echo 0-3 > /dev/cpuset/background/cpus
+  echo 0-3 > /dev/cpuset/system-background/cpus
+  echo 0-3 > /dev/cpuset/foreground/boost/cpus
+  echo 0-3 > /dev/cpuset/foreground/cpus
+  echo 0-3 > /dev/cpuset/android/cpus
+
+  # wait for network
+  (cd selfdrive/ui/spinner && exec ./spinner 'waiting for network...') & spin_pid=$!
+  until ping -W 1 -c 1 8.8.8.8; do sleep 1; done
+  kill $spin_pid
+
+  export PYTHONPATH="$PWD"
+
+  # start manager
+  cd selfdrive
+  ./manager.py
+
+  # if broken, keep on screen error
+  while true; do sleep 1; done
+}
+
+launch

selfdrive/boardd/boardd.cc

@@ -130,7 +130,11 @@ bool usb_connect() {
   #endif
 
   // no output is the default
-  libusb_control_transfer(dev_handle, 0x40, 0xdc, SAFETY_NOOUTPUT, 0, NULL, 0, TIMEOUT);
+  if (getenv("RECVMOCK")) {
+    libusb_control_transfer(dev_handle, 0x40, 0xdc, SAFETY_ELM327, 0, NULL, 0, TIMEOUT);
+  } else {
+    libusb_control_transfer(dev_handle, 0x40, 0xdc, SAFETY_NOOUTPUT, 0, NULL, 0, TIMEOUT);
+  }
 
   if (safety_setter_thread_handle == -1) {
     err = pthread_create(&safety_setter_thread_handle, NULL, safety_setter_thread, NULL);

selfdrive/can/libdbc_py.py

@@ -1,11 +1,12 @@
 import os
+import sys
 import subprocess
 
 from cffi import FFI
 
 can_dir = os.path.dirname(os.path.abspath(__file__))
 libdbc_fn = os.path.join(can_dir, "libdbc.so")
-subprocess.check_output(["make"], cwd=can_dir)
+subprocess.check_call(["make"], stdout=sys.stderr, cwd=can_dir)
 
 ffi = FFI()
 ffi.cdef("""

selfdrive/can/parser.cc

@@ -242,7 +242,7 @@ class CANParser {
       const auto& state = kv.second;
       if (state.check_threshold > 0 && (sec - state.seen) > state.check_threshold) {
         if (state.seen > 0) {
-          INFO("%X TIMEOUT\n", state.address);
+          DEBUG("%X TIMEOUT\n", state.address);
         }
         can_valid = false;
       }

selfdrive/car/__init__.py

@@ -6,12 +6,9 @@ from common.fingerprints import eliminate_incompatible_cars, all_known_cars
 
 from selfdrive.swaglog import cloudlog
 import selfdrive.messaging as messaging
-from .honda.interface import CarInterface as HondaInterface
-
-try:
-  from .toyota.interface import CarInterface as ToyotaInterface
-except ImportError:
-  ToyotaInterface = None
+from selfdrive.car.honda.interface import CarInterface as HondaInterface
+from selfdrive.car.toyota.interface import CarInterface as ToyotaInterface
+from selfdrive.car.mock.interface import CarInterface as MockInterface
 
 try:
   from .simulator.interface import CarInterface as SimInterface
@@ -31,9 +28,12 @@ interfaces = {
   "HONDA CR-V 2016 TOURING": HondaInterface,
   "TOYOTA PRIUS 2017": ToyotaInterface,
   "TOYOTA RAV4 2017": ToyotaInterface,
+  "TOYOTA RAV4 2017 HYBRID": ToyotaInterface,
 
   "simulator": SimInterface,
-  "simulator2": Sim2Interface
+  "simulator2": Sim2Interface,
+
+  "mock": MockInterface
 }
 
 # **** for use live only ****
@@ -75,12 +75,18 @@ def fingerprint(logcan, timeout):
   return (candidate_cars[0], finger)
 
 
-def get_car(logcan, sendcan=None, timeout=None):
+def get_car(logcan, sendcan=None, passive=True):
+
+  # TODO: timeout only useful for replays so controlsd can start before unlogger
+  timeout = 1. if passive else None
   candidate, fingerprints = fingerprint(logcan, timeout)
 
   if candidate is None:
     cloudlog.warning("car doesn't match any fingerprints: %r", fingerprints)
-    return None, None
+    if passive:
+      candidate = "mock"
+    else:
+      return None, None
 
   interface_cls = interfaces[candidate]
   params = interface_cls.get_params(candidate, fingerprints)

selfdrive/car/honda/carcontroller.py

@@ -122,7 +122,7 @@ class CarController(object):
     GAS_MAX = 1004
     BRAKE_MAX = 1024/4
     if CS.civic:
-      is_fw_modified = os.getenv("DONGLE_ID") in ['b0f5a01cf604185cxxx']
+      is_fw_modified = os.getenv("DONGLE_ID") in ['b0f5a01cf604185c']
       STEER_MAX = 0x1FFF if is_fw_modified else 0x1000
     elif CS.crv:
       STEER_MAX = 0x300  # CR-V only uses 12-bits and requires a lower value

selfdrive/car/honda/carstate.py

@@ -1,5 +1,4 @@
 import os
-import time
 from cereal import car
 from common.numpy_fast import interp
 import selfdrive.messaging as messaging

selfdrive/car/honda/interface.py

@@ -1,11 +1,11 @@
 #!/usr/bin/env python
 import os
-import time
 import numpy as np
 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 selfdrive.controls.lib.vehicle_model import VehicleModel
 from cereal import car
 from selfdrive.services import service_list
 import selfdrive.messaging as messaging
@@ -93,6 +93,7 @@ class CarInterface(object):
 
     # *** init the major players ***
     self.CS = CarState(CP)
+    self.VM = VehicleModel(CP)
 
     # sending if read only is False
     if sendcan is not None:
@@ -163,7 +164,7 @@ class CarInterface(object):
       ret.centerToFront = centerToFront_civic
       ret.steerRatio = 13.0
       # Civic at comma has modified steering FW, so different tuning for the Neo in that car
-      is_fw_modified = os.getenv("DONGLE_ID") in ['b0f5a01cf604185cxxx']
+      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.]
@@ -252,6 +253,8 @@ class CarInterface(object):
     ret.steerLimitAlert = True
     ret.startAccel = 0.5
 
+    ret.steerRateCost = 0.5
+
     return ret
 
   # returns a car.CarState
@@ -270,6 +273,7 @@ class CarInterface(object):
     ret.vEgo = self.CS.v_ego
     ret.aEgo = self.CS.a_ego
     ret.vEgoRaw = self.CS.v_ego_raw
+    ret.yawRate = self.VM.yaw_rate(self.CS.angle_steers * CV.DEG_TO_RAD, self.CS.v_ego)
     ret.standstill = self.CS.standstill
     ret.wheelSpeeds.fl = self.CS.v_wheel_fl
     ret.wheelSpeeds.fr = self.CS.v_wheel_fr
@@ -370,11 +374,11 @@ class CarInterface(object):
     else:
       self.can_invalid_count = 0
     if self.CS.steer_error:
-      events.append(create_event('steerUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
+      events.append(create_event('steerUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT]))
     elif self.CS.steer_not_allowed:
       events.append(create_event('steerTempUnavailable', [ET.NO_ENTRY, ET.WARNING]))
     if self.CS.brake_error:
-      events.append(create_event('brakeUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
+      events.append(create_event('brakeUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT]))
     if not ret.gearShifter == 'drive':
       events.append(create_event('wrongGear', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
     if not self.CS.door_all_closed:

selfdrive/car/mock/interface.py

@@ -0,0 +1,121 @@
+#!/usr/bin/env python
+import os
+import time
+import zmq
+from common.realtime import sec_since_boot
+import common.numpy_fast as np
+from cereal import car
+from selfdrive.config import Conversions as CV
+from selfdrive.services import service_list
+import selfdrive.messaging as messaging
+from selfdrive.controls.lib.drive_helpers import EventTypes as ET, create_event
+
+# mocked car interface to work with chffrplus
+TS = 0.01  # 100Hz
+YAW_FR = 0.2 # ~0.8s time constant on yaw rate filter
+# low pass gain
+LPG = 2 * 3.1415 * YAW_FR * TS / (1 + 2 * 3.1415 * YAW_FR * TS)
+
+
+class CarInterface(object):
+  def __init__(self, CP, sendcan=None):
+
+    self.CP = CP
+
+    print "Using Mock Car Interface"
+    context = zmq.Context()
+
+    # TODO: subscribe to phone sensor
+    self.sensor = messaging.sub_sock(context, service_list['sensorEvents'].port)
+    self.gps = messaging.sub_sock(context, service_list['gpsLocation'].port)
+
+    self.speed = 0.
+    self.yaw_rate = 0.
+    self.yaw_rate_meas = 0.
+
+  @staticmethod
+  def compute_gb(accel, speed):
+    return accel
+
+  @staticmethod
+  def calc_accel_override(a_ego, a_target, v_ego, v_target):
+    return 1.0
+
+  @staticmethod
+  def get_params(candidate, fingerprint):
+
+    ret = car.CarParams.new_message()
+
+    ret.carName = "mock"
+    ret.radarName = "mock"
+    ret.carFingerprint = candidate
+
+    ret.safetyModel = car.CarParams.SafetyModels.noOutput
+
+    # FIXME: hardcoding honda civic 2016 touring params so they can be used to
+    # scale unknown params for other cars
+    ret.mass = 1700.
+    ret.rotationalInertia = 2500.
+    ret.wheelbase = 2.70
+    ret.centerToFront = ret.wheelbase * 0.5
+    ret.steerRatio = 13. # reasonable
+    ret.longPidDeadzoneBP = [0.]
+    ret.longPidDeadzoneV = [0.]
+    ret.tireStiffnessFront = 1e6    # very stiff to neglect slip
+    ret.tireStiffnessRear = 1e6     # very stiff to neglect slip
+    ret.steerRatioRear = 0.
+
+    ret.steerMaxBP = [0.]
+    ret.steerMaxV = [0.]  # 2/3rd torque allowed above 45 kph
+    ret.gasMaxBP = [0.]
+    ret.gasMaxV = [0.]
+    ret.brakeMaxBP = [0.]
+    ret.brakeMaxV = [0.]
+
+    ret.longitudinalKpBP = [0.]
+    ret.longitudinalKpV = [0.]
+    ret.longitudinalKiBP = [0.]
+    ret.longitudinalKiV = [0.]
+
+    return ret
+
+  # returns a car.CarState
+  def update(self, c):
+
+    # get basic data from phone and gps since CAN isn't connected
+    sensors = messaging.recv_sock(self.sensor)
+    if sensors is not None:
+      for sensor in sensors.sensorEvents:
+        if sensor.type == 4:  # gyro
+          self.yaw_rate_meas = -sensor.gyro.v[0]
+
+    gps = messaging.recv_sock(self.gps)
+    if gps is not None:
+      self.speed = gps.gpsLocation.speed
+
+    # create message
+    ret = car.CarState.new_message()
+
+    # speeds
+    ret.vEgo = self.speed
+    ret.vEgoRaw = self.speed
+    ret.aEgo = 0.
+    self.yawRate = LPG * self.yaw_rate_meas + (1. - LPG) * self.yaw_rate
+    ret.yawRate = self.yaw_rate
+    ret.standstill = self.speed < 0.01
+    ret.wheelSpeeds.fl = self.speed
+    ret.wheelSpeeds.fr = self.speed
+    ret.wheelSpeeds.rl = self.speed
+    ret.wheelSpeeds.rr = self.speed
+    curvature = self.yaw_rate / max(self.speed, 1.)
+    ret.steeringAngle = curvature * self.CP.steerRatio * self.CP.wheelbase * CV.RAD_TO_DEG
+
+    events = []
+    #events.append(create_event('passive', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
+    ret.events = events
+
+    return ret.as_reader()
+
+  def apply(self, c):
+    # in mock no carcontrols
+    return False

selfdrive/car/toyota/carcontroller.py

@@ -6,7 +6,7 @@ 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_fcw_command
-from selfdrive.car.toyota.values import CAR, ECU
+from selfdrive.car.toyota.values import CAR, ECU, STATIC_MSGS
 
 
 ACCEL_HYST_GAP = 0.02  # don't change accel command for small oscilalitons within this value
@@ -27,60 +27,6 @@ TARGET_IDS = [0x340, 0x341, 0x342, 0x343, 0x344, 0x345,
               0x373, 0x374, 0x375, 0x380, 0x381, 0x382,
               0x383]
 
-# addr, [ecu, bus, 1/freq*100, vl]
-STATIC_MSGS = {0x141: (ECU.DSU, (CAR.PRIUS, CAR.RAV4), 1,   2, '\x00\x00\x00\x46'),
-               0x128: (ECU.DSU, (CAR.PRIUS, CAR.RAV4), 1,   3, '\xf4\x01\x90\x83\x00\x37'),
-
-               0x292: (ECU.APGS, (CAR.PRIUS, CAR.RAV4), 0,   3, '\x00\x00\x00\x00\x00\x00\x00\x9e'),
-               0x283: (ECU.DSU, (CAR.PRIUS, CAR.RAV4), 0,   3, '\x00\x00\x00\x00\x00\x00\x8c'),
-               0x2E6: (ECU.DSU, (CAR.PRIUS,), 0,   3, '\xff\xf8\x00\x08\x7f\xe0\x00\x4e'),
-               0x2E7: (ECU.DSU, (CAR.PRIUS,), 0,   3, '\xa8\x9c\x31\x9c\x00\x00\x00\x02'),
-
-               0x240: (ECU.CAM, (CAR.PRIUS, CAR.RAV4), 1,   5, '\x00\x10\x01\x00\x10\x01\x00'),
-               0x241: (ECU.CAM, (CAR.PRIUS, CAR.RAV4), 1,   5, '\x00\x10\x01\x00\x10\x01\x00'),
-               0x244: (ECU.CAM, (CAR.PRIUS, CAR.RAV4), 1,   5, '\x00\x10\x01\x00\x10\x01\x00'),
-               0x245: (ECU.CAM, (CAR.PRIUS, CAR.RAV4), 1,   5, '\x00\x10\x01\x00\x10\x01\x00'),
-               0x248: (ECU.CAM, (CAR.PRIUS, CAR.RAV4), 1,   5, '\x00\x00\x00\x00\x00\x00\x01'),
-               0x344: (ECU.DSU, (CAR.PRIUS, CAR.RAV4), 0,   5, '\x00\x00\x01\x00\x00\x00\x00\x50'),
-
-               0x160: (ECU.DSU, (CAR.PRIUS, CAR.RAV4), 1,   7, '\x00\x00\x08\x12\x01\x31\x9c\x51'),
-               0x161: (ECU.DSU, (CAR.PRIUS, CAR.RAV4), 1,   7, '\x00\x1e\x00\x00\x00\x80\x07'),
-
-               0x32E: (ECU.APGS, (CAR.PRIUS, CAR.RAV4), 0,  20, '\x00\x00\x00\x00\x00\x00\x00\x00'),
-               0x33E: (ECU.DSU, (CAR.PRIUS,), 0,  20, '\x0f\xff\x26\x40\x00\x1f\x00'),
-               0x365: (ECU.DSU, (CAR.PRIUS,), 0,  20, '\x00\x00\x00\x80\x03\x00\x08'),
-               0x365: (ECU.DSU, (CAR.RAV4,), 0,  20, '\x00\x00\x00\x80\xfc\x00\x08'),
-               0x366: (ECU.DSU, (CAR.PRIUS,), 0,  20, '\x00\x00\x4d\x82\x40\x02\x00'),
-               0x366: (ECU.DSU, (CAR.RAV4,), 0,  20, '\x00\x72\x07\xff\x09\xfe\x00'),
-
-               0x367: (ECU.CAM, (CAR.PRIUS, CAR.RAV4), 0,  40, '\x06\x00'),
-
-               0x414: (ECU.CAM, (CAR.PRIUS, CAR.RAV4), 0, 100, '\x00\x00\x00\x00\x00\x00\x17\x00'),
-               0x489: (ECU.CAM, (CAR.PRIUS, CAR.RAV4), 0, 100, '\x00\x00\x00\x00\x00\x00\x00'),
-               0x48a: (ECU.CAM, (CAR.PRIUS, CAR.RAV4), 0, 100, '\x00\x00\x00\x00\x00\x00\x00'),
-               0x48b: (ECU.CAM, (CAR.PRIUS, CAR.RAV4), 0, 100, '\x66\x06\x08\x0a\x02\x00\x00\x00'),
-               0x4d3: (ECU.CAM, (CAR.PRIUS, CAR.RAV4), 0, 100, '\x1C\x00\x00\x01\x00\x00\x00\x00'),
-               0x130: (ECU.CAM, (CAR.PRIUS, CAR.RAV4), 1, 100, '\x00\x00\x00\x00\x00\x00\x38'),
-               0x466: (ECU.CAM, (CAR.PRIUS, CAR.RAV4), 1, 100, '\x20\x20\xAD'),
-               0x396: (ECU.APGS, (CAR.PRIUS, CAR.RAV4), 0, 100, '\xBD\x00\x00\x00\x60\x0F\x02\x00'),
-               0x43A: (ECU.APGS, (CAR.PRIUS, CAR.RAV4), 0, 100, '\x84\x00\x00\x00\x00\x00\x00\x00'),
-               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'),
-               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'),
-              }
-
-
-def check_ecu_msgs(fingerprint, candidate, ecu):
-  # return True if fingerprint contains messages normally sent by a given ecu
-  ecu_msgs = [x for x in STATIC_MSGS if (ecu == STATIC_MSGS[x][0] and 
-                                         candidate in STATIC_MSGS[x][1] and 
-                                         STATIC_MSGS[x][2] == 0)]
-
-  return any(msg for msg in fingerprint if msg in ecu_msgs)
-
-
 def accel_hysteresis(accel, accel_steady, enabled):
 
   # for small accel oscillations within ACCEL_HYST_GAP, don't change the accel command

selfdrive/car/toyota/carstate.py

@@ -1,5 +1,4 @@
 import os
-import time
 import selfdrive.messaging as messaging
 from selfdrive.car.toyota.values import CAR
 from selfdrive.can.parser import CANParser
@@ -19,7 +18,7 @@ def parse_gear_shifter(can_gear, car_fingerprint):
       return "drive"
     elif can_gear == 0x4:
       return "brake"
-  elif car_fingerprint == CAR.RAV4:
+  elif car_fingerprint in [CAR.RAV4, CAR.RAV4H]:
     if can_gear == 0x20:
       return "park"
     elif can_gear == 0x10:
@@ -47,6 +46,17 @@ def get_can_parser(CP):
       (550, 40),
       (581, 33)
     ]
+  elif CP.carFingerprint == CAR.RAV4H:
+    dbc_f = 'toyota_rav4_hybrid_2017_pt.dbc'
+    signals = [
+      ("GEAR", 956, 0),
+      ("BRAKE_PRESSED", 550, 0),
+      ("GAS_PEDAL", 581, 0),
+    ]
+    checks = [
+      (550, 40),
+      (581, 33)
+    ]
   elif CP.carFingerprint == CAR.RAV4:
     dbc_f = 'toyota_rav4_2017_pt.dbc'
     signals = [
@@ -131,6 +141,10 @@ class CarState(object):
       can_gear = cp.vl[295]['GEAR']
       self.brake_pressed = cp.vl[550]['BRAKE_PRESSED']
       self.pedal_gas = cp.vl[581]['GAS_PEDAL']
+    elif self.car_fingerprint == CAR.RAV4H:
+      can_gear = cp.vl[956]['GEAR']
+      self.brake_pressed = cp.vl[550]['BRAKE_PRESSED']
+      self.pedal_gas = cp.vl[581]['GAS_PEDAL']
     elif self.car_fingerprint == CAR.RAV4:
       can_gear = cp.vl[956]['GEAR']
       self.brake_pressed = cp.vl[548]['BRAKE_PRESSED']

selfdrive/car/toyota/interface.py

@@ -1,20 +1,24 @@
 #!/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, 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.config import Conversions as CV
 from selfdrive.services import service_list
 import selfdrive.messaging as messaging
 from selfdrive.controls.lib.drive_helpers import EventTypes as ET, create_event
+from selfdrive.controls.lib.vehicle_model import VehicleModel
+from selfdrive.car.toyota.carstate import CarState, get_can_parser
+from selfdrive.car.toyota.values import CAR, ECU, check_ecu_msgs
+try:
+  from selfdrive.car.toyota.carcontroller import CarController
+except ImportError:
+  CarController = None
 
 class CarInterface(object):
   def __init__(self, CP, sendcan=None):
     self.CP = CP
+    self.VM = VehicleModel(CP)
 
     self.frame = 0
     self.can_invalid_count = 0
@@ -70,9 +74,8 @@ class CarInterface(object):
     tireStiffnessFront_civic = 85400
     tireStiffnessRear_civic = 90000
 
-    stop_and_go = True
     ret.mass = 3045./2.205 + std_cargo
-    ret.wheelbase = 2.70
+    ret.wheelbase = 2.70 if candidate == CAR.PRIUS else 2.65
     ret.centerToFront = ret.wheelbase * 0.44
     ret.steerRatio = 14.5 #Rav4 2017, TODO: find exact value for Prius
     ret.steerKp, ret.steerKi = 0.6, 0.05
@@ -83,9 +86,9 @@ class CarInterface(object):
 
     # min speed to enable ACC. if car can do stop and go, then set enabling speed
     # to a negative value, so it won't matter.
-    if candidate == CAR.PRIUS:
+    if candidate in [CAR.PRIUS, CAR.RAV4H]: # rav4 hybrid can do stop and go
       ret.minEnableSpeed = -1.
-    elif candidate == CAR.RAV4:   # TODO: hack Rav4 to do stop and go
+    elif candidate == CAR.RAV4:   # TODO: hack ICE Rav4 to do stop and go
       ret.minEnableSpeed = 19. * CV.MPH_TO_MS
 
     centerToRear = ret.wheelbase - ret.centerToFront
@@ -131,6 +134,11 @@ class CarInterface(object):
     ret.longitudinalKiBP = [0., 35.]
     ret.longitudinalKiV = [0.54, 0.36]
 
+    if candidate == CAR.PRIUS:
+      ret.steerRateCost = 2.
+    elif candidate in [CAR.RAV4, CAR.RAV4H]:
+      ret.steerRateCost = 1.
+
     return ret
 
   # returns a car.CarState
@@ -150,6 +158,7 @@ class CarInterface(object):
     ret.vEgo = self.CS.v_ego
     ret.vEgoRaw = self.CS.v_ego_raw
     ret.aEgo = self.CS.a_ego
+    ret.yawRate = self.VM.yaw_rate(self.CS.angle_steers * CV.DEG_TO_RAD, self.CS.v_ego)
     ret.standstill = self.CS.standstill
     ret.wheelSpeeds.fl = self.CS.v_wheel_fl
     ret.wheelSpeeds.fr = self.CS.v_wheel_fr
@@ -180,7 +189,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 = 0.
-    ret.cruiseState.standstill = self.CS.pcm_acc_status == 7
+    if self.CP.carFingerprint == CAR.RAV4H:
+      # ignore standstill in hybrid rav4, since pcm allows to restart without
+      # receiving any special command
+      ret.cruiseState.standstill = False
+    else:
+      ret.cruiseState.standstill = self.CS.pcm_acc_status == 7  
 
     # TODO: button presses
     buttonEvents = []
@@ -223,8 +237,8 @@ class CarInterface(object):
       events.append(create_event('reverseGear', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
     if self.CS.steer_error:
       events.append(create_event('steerTempUnavailable', [ET.NO_ENTRY, ET.WARNING]))
-    if self.CS.low_speed_lockout:
-      events.append(create_event('lowSpeedLockout', [ET.NO_ENTRY]))
+    if self.CS.low_speed_lockout and self.CP.enableDsu:
+      events.append(create_event('lowSpeedLockout', [ET.NO_ENTRY, ET.PERMANENT]))
     if ret.vEgo < self.CP.minEnableSpeed and self.CP.enableDsu:
       events.append(create_event('speedTooLow', [ET.NO_ENTRY]))
       if c.actuators.gas > 0.1:

selfdrive/car/toyota/values.py

@@ -1,8 +1,64 @@
 class CAR: 
-  PRIUS = "TOYOTA PRIUS 2017" 
-  RAV4 = "TOYOTA RAV4 2017" 
- 
+  PRIUS = "TOYOTA PRIUS 2017"
+  RAV4H = "TOYOTA RAV4 2017 HYBRID"
+  RAV4 = "TOYOTA RAV4 2017"
+
+
 class ECU: 
   CAM = 0 # camera 
   DSU = 1 # driving support unit 
   APGS = 2 # advanced parking guidance system 
+
+
+# addr, [ecu, bus, 1/freq*100, vl]
+STATIC_MSGS = {0x141: (ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 1,   2, '\x00\x00\x00\x46'),
+               0x128: (ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 1,   3, '\xf4\x01\x90\x83\x00\x37'),
+
+               0x292: (ECU.APGS, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 0,   3, '\x00\x00\x00\x00\x00\x00\x00\x9e'),
+               0x283: (ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 0,   3, '\x00\x00\x00\x00\x00\x00\x8c'),
+               0x2E6: (ECU.DSU, (CAR.PRIUS, CAR.RAV4H,), 0,   3, '\xff\xf8\x00\x08\x7f\xe0\x00\x4e'),
+               0x2E7: (ECU.DSU, (CAR.PRIUS, CAR.RAV4H,), 0,   3, '\xa8\x9c\x31\x9c\x00\x00\x00\x02'),
+
+               0x240: (ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 1,   5, '\x00\x10\x01\x00\x10\x01\x00'),
+               0x241: (ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 1,   5, '\x00\x10\x01\x00\x10\x01\x00'),
+               0x244: (ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 1,   5, '\x00\x10\x01\x00\x10\x01\x00'),
+               0x245: (ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 1,   5, '\x00\x10\x01\x00\x10\x01\x00'),
+               0x248: (ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 1,   5, '\x00\x00\x00\x00\x00\x00\x01'),
+               0x344: (ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 0,   5, '\x00\x00\x01\x00\x00\x00\x00\x50'),
+
+               0x160: (ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 1,   7, '\x00\x00\x08\x12\x01\x31\x9c\x51'),
+               0x161: (ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 1,   7, '\x00\x1e\x00\x00\x00\x80\x07'),
+
+               0x32E: (ECU.APGS, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 0,  20, '\x00\x00\x00\x00\x00\x00\x00\x00'),
+               0x33E: (ECU.DSU, (CAR.PRIUS, CAR.RAV4H,), 0,  20, '\x0f\xff\x26\x40\x00\x1f\x00'),
+               0x365: (ECU.DSU, (CAR.PRIUS, CAR.RAV4H,), 0,  20, '\x00\x00\x00\x80\x03\x00\x08'),
+               0x365: (ECU.DSU, (CAR.RAV4,), 0,  20, '\x00\x00\x00\x80\xfc\x00\x08'),
+               0x366: (ECU.DSU, (CAR.PRIUS, CAR.RAV4H,), 0,  20, '\x00\x00\x4d\x82\x40\x02\x00'),
+               0x366: (ECU.DSU, (CAR.RAV4,), 0,  20, '\x00\x72\x07\xff\x09\xfe\x00'),
+
+               0x367: (ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 0,  40, '\x06\x00'),
+
+               0x414: (ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 0, 100, '\x00\x00\x00\x00\x00\x00\x17\x00'),
+               0x489: (ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 0, 100, '\x00\x00\x00\x00\x00\x00\x00'),
+               0x48a: (ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 0, 100, '\x00\x00\x00\x00\x00\x00\x00'),
+               0x48b: (ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 0, 100, '\x66\x06\x08\x0a\x02\x00\x00\x00'),
+               0x4d3: (ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 0, 100, '\x1C\x00\x00\x01\x00\x00\x00\x00'),
+               0x130: (ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 1, 100, '\x00\x00\x00\x00\x00\x00\x38'),
+               0x466: (ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 1, 100, '\x20\x20\xAD'),
+               0x396: (ECU.APGS, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 0, 100, '\xBD\x00\x00\x00\x60\x0F\x02\x00'),
+               0x43A: (ECU.APGS, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 0, 100, '\x84\x00\x00\x00\x00\x00\x00\x00'),
+               0x43B: (ECU.APGS, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 0, 100, '\x00\x00\x00\x00\x00\x00\x00\x00'),
+               0x497: (ECU.APGS, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 0, 100, '\x00\x00\x00\x00\x00\x00\x00\x00'),
+               0x4CC: (ECU.APGS, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 0, 100, '\x0D\x00\x00\x00\x00\x00\x00\x00'),
+               0x4CB: (ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 0, 100, '\x0c\x00\x00\x00\x00\x00\x00\x00'),
+               0x470: (ECU.DSU, (CAR.PRIUS, CAR.RAV4H,), 1, 100, '\x00\x00\x02\x7a'),
+              }
+
+
+def check_ecu_msgs(fingerprint, candidate, ecu):
+  # return True if fingerprint contains messages normally sent by a given ecu
+  ecu_msgs = [x for x in STATIC_MSGS if (ecu == STATIC_MSGS[x][0] and 
+                                         candidate in STATIC_MSGS[x][1] and 
+                                         STATIC_MSGS[x][2] == 0)]
+
+  return any(msg for msg in fingerprint if msg in ecu_msgs)

selfdrive/common/version.h

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

selfdrive/controls/controlsd.py

@@ -2,7 +2,7 @@
 import json
 from copy import copy
 import zmq
-from cereal import car
+from cereal import car, log
 from common.numpy_fast import clip
 from common.realtime import sec_since_boot, set_realtime_priority, Ratekeeper
 from common.profiler import Profiler
@@ -30,6 +30,7 @@ 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
 
+State = log.Live100Data.ControlState
 
 class Calibration:
   UNCALIBRATED = 0
@@ -37,38 +38,43 @@ class Calibration:
   INVALID = 2
 
 
-class State:
-  DISABLED = 'disabled'
-  ENABLED = 'enabled'
-  PRE_ENABLED = 'preEnabled'
-  SOFT_DISABLING = 'softDisabling'
-
-
 # True when actuators are controlled
 def isActive(state):
-  return state in [State.ENABLED, State.SOFT_DISABLING]
+  return state in [State.enabled, State.softDisabling]
 
 
 # True if system is engaged
 def isEnabled(state):
-  return (isActive(state) or state == State.PRE_ENABLED)
+  return (isActive(state) or state == State.preEnabled)
 
 
-def data_sample(CI, CC, thermal, health, cal, cal_status, overtemp, free_space):
+def data_sample(CI, CC, thermal, calibration, health, poller, cal_status, overtemp, free_space):
 
   # *** read can and compute car states ***
   CS = CI.update(CC)
   events = list(CS.events)
 
+  td = None
+  cal = None
+  hh = None
+
+  for socket, event in poller.poll(0):
+    if socket is thermal:
+      td = messaging.recv_one(socket)
+    elif socket is calibration:
+      cal = messaging.recv_one(socket)
+    elif socket is health:
+      hh = messaging.recv_one(socket)
+
   # *** thermal checking logic ***
   # thermal data, checked every second
-  td = messaging.recv_sock(thermal)
   if td is not None:
-    # overtemp above 95 deg
-    overtemp = any(
-        t > 950
-        for t in (td.thermal.cpu0, td.thermal.cpu1, td.thermal.cpu2,
-                  td.thermal.cpu3, td.thermal.mem, td.thermal.gpu))
+    # CPU overtemp above 95 deg
+    overtemp_proc = any(t > 950 for t in
+                        (td.thermal.cpu0, td.thermal.cpu1, td.thermal.cpu2,
+                         td.thermal.cpu3, td.thermal.mem, td.thermal.gpu))
+    overtemp_bat = td.thermal.bat > 50000 # 50c
+    overtemp = overtemp_proc or overtemp_bat
 
     # under 15% of space free no enable allowed
     free_space = td.thermal.freeSpace < 0.15
@@ -80,7 +86,6 @@ def data_sample(CI, CC, thermal, health, cal, cal_status, overtemp, free_space):
     events.append(create_event('outOfSpace', [ET.NO_ENTRY]))
 
   # *** read calibration status ***
-  cal = messaging.recv_sock(cal)
   if cal is not None:
     cal_status = cal.liveCalibration.calStatus
 
@@ -91,11 +96,10 @@ def data_sample(CI, CC, thermal, health, cal, cal_status, overtemp, free_space):
       events.append(create_event('calibrationInvalid', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
 
   # *** health checking logic ***
-  hh = messaging.recv_sock(health)
   if hh is not None:
     controls_allowed = hh.health.controlsAllowed
     if not controls_allowed:
-      events.append(create_event('controlsMismatch', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
+      events.append(create_event('controlsMismatch', [ET.IMMEDIATE_DISABLE]))
 
   return CS, events, cal_status, overtemp, free_space
 
@@ -123,6 +127,7 @@ def state_transition(CS, CP, state, events, soft_disable_timer, v_cruise_kph, AM
 
   # handle button presses. TODO: this should be in state_control, but a decelCruise press
   # would have the effect of both enabling and changing speed is checked after the state transition
+  v_cruise_kph_last = v_cruise_kph
   for b in CS.buttonEvents:
     if not CP.enableCruise and enabled and not b.pressed:
       if b.type == "accelCruise":
@@ -138,76 +143,75 @@ def state_transition(CS, CP, state, events, soft_disable_timer, v_cruise_kph, AM
   # ***** handle state transitions *****
 
   # DISABLED
-  if state == State.DISABLED:
+  if state == State.disabled:
     if get_events(events, [ET.ENABLE]):
-      if get_events(events, [ET.NO_ENTRY, ET.SOFT_DISABLE, ET.IMMEDIATE_DISABLE]):
-        for e in get_events(events, [ET.SOFT_DISABLE, ET.IMMEDIATE_DISABLE]):
-          AM.add(e, enabled)
+      if get_events(events, [ET.NO_ENTRY]):
         for e in get_events(events, [ET.NO_ENTRY]):
           AM.add(str(e) + "NoEntry", enabled)
+
       else:
         if get_events(events, [ET.PRE_ENABLE]):
-          state = State.PRE_ENABLED
+          state = State.preEnabled
         else:
-          state = State.ENABLED
+          state = State.enabled
         AM.add("enable", enabled)
         # on activation, let's always set v_cruise from where we are, even if PCM ACC is active
         v_cruise_kph = int(round(max(CS.vEgo * CV.MS_TO_KPH, V_CRUISE_ENABLE_MIN)))
 
   # ENABLED
-  elif state == State.ENABLED:
+  elif state == State.enabled:
     if get_events(events, [ET.USER_DISABLE]):
-      state = State.DISABLED
+      state = State.disabled
       AM.add("disable", enabled)
 
     elif get_events(events, [ET.IMMEDIATE_DISABLE]):
-      state = State.DISABLED
+      state = State.disabled
       for e in get_events(events, [ET.IMMEDIATE_DISABLE]):
         AM.add(e, enabled)
 
     elif get_events(events, [ET.SOFT_DISABLE]):
-      state = State.SOFT_DISABLING
+      state = State.softDisabling
       soft_disable_timer = 300   # 3s TODO: use rate
       for e in get_events(events, [ET.SOFT_DISABLE]):
         AM.add(e, enabled)
 
   # SOFT DISABLING
-  elif state == State.SOFT_DISABLING:
+  elif state == State.softDisabling:
     if get_events(events, [ET.USER_DISABLE]):
-      state = State.DISABLED
+      state = State.disabled
       AM.add("disable", enabled)
 
     elif get_events(events, [ET.IMMEDIATE_DISABLE]):
-      state = State.DISABLED
+      state = State.disabled
       for e in get_events(events, [ET.IMMEDIATE_DISABLE]):
         AM.add(e, enabled)
 
     elif not get_events(events, [ET.SOFT_DISABLE]):
       # no more soft disabling condition, so go back to ENABLED
-      state = State.ENABLED
+      state = State.enabled
 
     elif soft_disable_timer <= 0:
-      state = State.DISABLED
+      state = State.disabled
 
-  # TODO: PRE ENABLING
-  elif state == State.PRE_ENABLED:
+  # PRE ENABLING
+  elif state == State.preEnabled:
     if get_events(events, [ET.USER_DISABLE]):
-      state = State.DISABLED
+      state = State.disabled
       AM.add("disable", enabled)
 
     elif get_events(events, [ET.IMMEDIATE_DISABLE, ET.SOFT_DISABLE]):
-      state = State.DISABLED
+      state = State.disabled
       for e in get_events(events, [ET.IMMEDIATE_DISABLE, ET.SOFT_DISABLE]):
         AM.add(e, enabled)
 
     elif not get_events(events, [ET.PRE_ENABLE]):
-      state = State.ENABLED
+      state = State.enabled
 
-  return state, soft_disable_timer, v_cruise_kph
+  return state, soft_disable_timer, v_cruise_kph, v_cruise_kph_last
 
 
-def state_control(plan, CS, CP, state, events, v_cruise_kph, AM, rk, awareness_status,
-                  PL, LaC, LoC, VM, angle_offset, rear_view_allowed, rear_view_toggle):
+def state_control(plan, CS, CP, state, events, v_cruise_kph, v_cruise_kph_last, AM, rk, 
+                  awareness_status, PL, LaC, LoC, VM, angle_offset, rear_view_allowed, rear_view_toggle):
   # Given the state, this function returns the actuators
 
   # reset actuators to zero
@@ -217,9 +221,6 @@ def state_control(plan, CS, CP, state, events, v_cruise_kph, AM, rk, awareness_s
   active = isActive(state)
 
   for b in CS.buttonEvents:
-    # any button event resets awarenesss_status
-    awareness_status = 1.
-
     # button presses for rear view
     if b.type == "leftBlinker" or b.type == "rightBlinker":
       if b.pressed and rear_view_allowed:
@@ -238,24 +239,20 @@ def state_control(plan, CS, CP, state, events, v_cruise_kph, AM, rk, awareness_s
   # ***** state specific actions *****
 
   # DISABLED
-  if state in [State.PRE_ENABLED, State.DISABLED]:
-    awareness_status = 1.
+  if state in [State.preEnabled, State.disabled]:
+
     LaC.reset()
     LoC.reset(v_pid=CS.vEgo)
 
   # ENABLED or SOFT_DISABLING
-  elif state in [State.ENABLED, State.SOFT_DISABLING]:
-
-    if CS.steeringPressed:
-      # reset awareness status on steering
-      awareness_status = 1.0
+  elif state in [State.enabled, State.softDisabling]:
 
-    # 6 minutes driver you're on
+    # decrease awareness status
     awareness_status -= 0.01/(AWARENESS_TIME)
     if awareness_status <= 0.:
       AM.add("driverDistracted", enabled)
     elif awareness_status <= AWARENESS_PRE_TIME / AWARENESS_TIME and \
-         awareness_status >= (AWARENESS_PRE_TIME - 0.1) / AWARENESS_TIME:
+         awareness_status >= (AWARENESS_PRE_TIME - 4.) / AWARENESS_TIME:
       AM.add("preDriverDistracted", enabled)
 
     # parse warnings from car specific interface
@@ -286,6 +283,17 @@ def state_control(plan, CS, CP, state, events, v_cruise_kph, AM, rk, awareness_s
   if CP.enableCruise and CS.cruiseState.enabled:
     v_cruise_kph = CS.cruiseState.speed * CV.MS_TO_KPH
 
+  # reset conditions for the 6 minutes timout
+  if CS.buttonEvents or \
+     v_cruise_kph != v_cruise_kph_last or \
+     CS.steeringPressed or \
+     state in [State.preEnabled, State.disabled]:
+    awareness_status = 1.
+
+  # parse permanent warnings to display constantly
+  for e in get_events(events, [ET.PERMANENT]):
+    AM.add(str(e) + "Permanent", enabled)
+
   # *** process alerts ***
 
   AM.process_alerts(sec_since_boot())
@@ -293,7 +301,7 @@ def state_control(plan, CS, CP, state, events, v_cruise_kph, AM, rk, awareness_s
   return actuators, v_cruise_kph, awareness_status, angle_offset, rear_view_toggle
 
 
-def data_send(plan, plan_ts, CS, CI, CP, state, events, actuators, v_cruise_kph, rk, carstate,
+def data_send(plan, plan_ts, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, carstate,
               carcontrol, live100, livempc, AM, rear_view_allowed, rear_view_toggle, awareness_status,
               LaC, LoC, angle_offset, passive):
 
@@ -335,6 +343,8 @@ def data_send(plan, plan_ts, CS, CI, CP, state, events, actuators, v_cruise_kph,
   dat.live100.rearViewCam = ('reverseGear' in [e.name for e in events] and rear_view_allowed) or rear_view_toggle
   dat.live100.alertText1 = AM.alert_text_1
   dat.live100.alertText2 = AM.alert_text_2
+  dat.live100.alertSize = AM.alert_size
+  dat.live100.alertStatus = AM.alert_status
   dat.live100.awarenessStatus = max(awareness_status, 0.0) if isEnabled(state) else 0.0
 
   # what packets were used to process
@@ -349,6 +359,7 @@ def data_send(plan, plan_ts, CS, CI, CP, state, events, actuators, v_cruise_kph,
   dat.live100.vEgo = CS.vEgo
   dat.live100.vEgoRaw = CS.vEgoRaw
   dat.live100.angleSteers = CS.steeringAngle
+  dat.live100.curvature = VM.calc_curvature(CS.steeringAngle * CV.DEG_TO_RAD, CS.vEgo)
   dat.live100.steerOverride = CS.steeringPressed
 
   # high level control state
@@ -410,7 +421,7 @@ def data_send(plan, plan_ts, CS, CI, CP, state, events, actuators, v_cruise_kph,
 
 def controlsd_thread(gctx, rate=100):
   # start the loop
-  set_realtime_priority(2)
+  set_realtime_priority(3)
 
   context = zmq.Context()
 
@@ -429,9 +440,11 @@ def controlsd_thread(gctx, rate=100):
     sendcan = None
 
   # sub
-  thermal = messaging.sub_sock(context, service_list['thermal'].port)
-  health = messaging.sub_sock(context, service_list['health'].port)
-  cal = messaging.sub_sock(context, service_list['liveCalibration'].port)
+  poller = zmq.Poller()
+  thermal = messaging.sub_sock(context, service_list['thermal'].port, conflate=True, poller=poller)
+  health = messaging.sub_sock(context, service_list['health'].port, conflate=True, poller=poller)
+  cal = messaging.sub_sock(context, service_list['liveCalibration'].port, conflate=True, poller=poller)
+
   logcan = messaging.sub_sock(context, service_list['can'].port)
 
   CC = car.CarControl.new_message()
@@ -439,10 +452,7 @@ def controlsd_thread(gctx, rate=100):
   CI, CP = get_car(logcan, sendcan, 1.0 if passive else None)
 
   if CI is None:
-    if passive:
-      return
-    else:
-      raise Exception("unsupported car")
+    raise Exception("unsupported car")
 
   if passive:
     CP.safetyModel = car.CarParams.SafetyModels.noOutput
@@ -461,7 +471,7 @@ def controlsd_thread(gctx, rate=100):
   # write CarParams
   params.put("CarParams", CP.to_bytes())
 
-  state = State.DISABLED
+  state = State.disabled
   soft_disable_timer = 0
   v_cruise_kph = 255
   overtemp = False
@@ -472,6 +482,7 @@ def controlsd_thread(gctx, rate=100):
 
   # 0.0 - 1.0
   awareness_status = 1.
+  v_cruise_kph_last = 0
 
   rk = Ratekeeper(rate, print_delay_threshold=2./1000)
 
@@ -485,14 +496,14 @@ def controlsd_thread(gctx, rate=100):
     except (ValueError, KeyError):
       pass
 
-  prof = Profiler()
+  prof = Profiler(False)  # off by default
 
   while 1:
 
-    prof.reset()  # avoid memory leak
+    prof.checkpoint("Ratekeeper", ignore=True)  # rk is here
 
     # sample data and compute car events
-    CS, events, cal_status, overtemp, free_space = data_sample(CI, CC, thermal, health, cal, cal_status,
+    CS, events, cal_status, overtemp, free_space = data_sample(CI, CC, thermal, cal, health, poller, cal_status,
                                                                overtemp, free_space)
     prof.checkpoint("Sample")
 
@@ -502,24 +513,26 @@ def controlsd_thread(gctx, rate=100):
 
     if not passive:
       # update control state
-      state, soft_disable_timer, v_cruise_kph = state_transition(CS, CP, state, events, soft_disable_timer, v_cruise_kph, AM)
+      state, soft_disable_timer, v_cruise_kph, v_cruise_kph_last = state_transition(CS, CP, state, events, soft_disable_timer, 
+                                                                                    v_cruise_kph, AM)
       prof.checkpoint("State transition")
 
     # compute actuators
     actuators, v_cruise_kph, awareness_status, angle_offset, rear_view_toggle = state_control(plan, CS, CP, state, events, v_cruise_kph,
-                                                                            AM, rk, awareness_status, PL, LaC, LoC, VM, angle_offset,
-                                                                            rear_view_allowed, rear_view_toggle)
+                                                                            v_cruise_kph_last, AM, rk, awareness_status, PL, LaC, LoC, VM,
+                                                                            angle_offset, rear_view_allowed, rear_view_toggle)
     prof.checkpoint("State Control")
 
     # publish data
-    CC = data_send(plan, plan_ts, CS, CI, CP, state, events, actuators, v_cruise_kph,
+    CC = data_send(plan, plan_ts, CS, CI, CP, VM, state, events, actuators, v_cruise_kph,
                    rk, carstate, carcontrol, live100, livempc, AM, rear_view_allowed,
                    rear_view_toggle, awareness_status, LaC, LoC, angle_offset, passive)
     prof.checkpoint("Sent")
 
     # *** run loop at fixed rate ***
-    if rk.keep_time():
-      prof.display()
+    rk.keep_time()
+
+    prof.display()
 
 
 def main(gctx=None):

selfdrive/controls/lib/alertmanager.py

@@ -1,19 +1,25 @@
-from cereal import car
+from cereal import car, log
 from selfdrive.swaglog import cloudlog
+from common.realtime import sec_since_boot
 import copy
 
 
 # Priority
-class PT:
+class Priority:
   HIGH = 3
   MID = 2
   LOW = 1
+  LOWEST = 0
 
+AlertSize = log.Live100Data.AlertSize
+AlertStatus = log.Live100Data.AlertStatus
 
 class Alert(object):
   def __init__(self, 
                alert_text_1,
                alert_text_2,
+               alert_status,
+               alert_size,
                alert_priority,
                visual_alert,
                audible_alert, 
@@ -23,6 +29,8 @@ class Alert(object):
 
     self.alert_text_1 = alert_text_1
     self.alert_text_2 = alert_text_2
+    self.alert_status = alert_status
+    self.alert_size = alert_size
     self.alert_priority = alert_priority
     self.visual_alert = visual_alert if visual_alert is not None else "none"
     self.audible_alert = audible_alert if audible_alert is not None else "none"
@@ -31,6 +39,8 @@ class Alert(object):
     self.duration_hud_alert = duration_hud_alert
     self.duration_text = duration_text
 
+    self.start_time = 0.
+
     # typecheck that enums are valid on startup
     tst = car.CarControl.new_message()
     tst.hudControl.visualAlert = self.visual_alert
@@ -51,347 +61,418 @@ class AlertManager(object):
     "enable": Alert(
         "",
         "",
-        PT.MID, None, "beepSingle", .2, 0., 0.),
+        AlertStatus.normal, AlertSize.none,
+        Priority.MID, None, "beepSingle", .2, 0., 0.),
 
     "disable": Alert(
         "",
         "",
-        PT.MID, None, "beepSingle", .2, 0., 0.),
+        AlertStatus.normal, AlertSize.none,
+        Priority.MID, None, "beepSingle", .2, 0., 0.),
 
     "fcw": Alert(
-        "Brake", 
+        "Brake!", 
         "Risk of Collision", 
-        PT.HIGH, "fcw", "chimeRepeated", 1., 2., 2.),
+        AlertStatus.critical, AlertSize.full,
+        Priority.HIGH, "fcw", "chimeRepeated", 1., 2., 2.),
 
     "steerSaturated": Alert(
         "Take Control", 
         "Turn Exceeds Limit", 
-        PT.LOW, "steerRequired", "chimeSingle", 1., 2., 3.),
+        AlertStatus.userPrompt, AlertSize.full,
+        Priority.LOW, "steerRequired", "chimeSingle", 1., 2., 3.),
 
     "steerTempUnavailable": Alert(
         "Take Control", 
         "Steer Temporarily Unavailable", 
-        PT.LOW, "steerRequired", "chimeDouble", .4, 2., 3.),
+        AlertStatus.userPrompt, AlertSize.full,
+        Priority.LOW, "steerRequired", "chimeDouble", .4, 2., 3.),
 
     "preDriverDistracted": Alert(
         "Take Control", 
         "User Distracted", 
-        PT.LOW, "steerRequired", "chimeDouble", .4, 2., 3.),
+        AlertStatus.userPrompt, AlertSize.full,
+        Priority.LOW, "steerRequired", "chimeDouble", .1, .1, .1),
 
     "driverDistracted": Alert(
         "Take Control to Regain Speed", 
         "User Distracted", 
-        PT.LOW, "steerRequired", "chimeRepeated", .5, .5, .5),
+        AlertStatus.critical, AlertSize.full,
+        Priority.MID, "steerRequired", "chimeRepeated", .1, .1, .1),
 
     "startup": Alert(
         "Always Keep Hands on Wheel", 
         "Be Ready to Take Over Any Time", 
-        PT.LOW, None, None, 0., 0., 15.),
+        AlertStatus.normal, AlertSize.full,
+        Priority.LOWEST, None, None, 0., 0., 15.),
 
     "ethicalDilemma": Alert(
         "Take Control Immediately", 
         "Ethical Dilemma Detected", 
-        PT.HIGH, "steerRequired", "chimeRepeated", 1., 3., 3.),
+        AlertStatus.critical, AlertSize.full,
+        Priority.HIGH, "steerRequired", "chimeRepeated", 1., 3., 3.),
 
     "steerTempUnavailableNoEntry": Alert(
         "Comma Unavailable", 
         "Steer Temporary Unavailable", 
-        PT.LOW, None, "chimeDouble", .4, 0., 3.),
+        AlertStatus.normal, AlertSize.full,
+        Priority.LOW, None, "chimeDouble", .4, 0., 3.),
 
     "manualRestart": Alert(
         "Take Control",
         "Resume Driving Manually",
-        PT.LOW, None, None, .0, 0., 1.),
+        AlertStatus.userPrompt, AlertSize.full,
+        Priority.LOW, None, None, 0., 0., .2),
 
     # Non-entry only alerts
     "wrongCarModeNoEntry": Alert(
         "Comma Unavailable", 
         "Main Switch Off", 
-        PT.LOW, None, "chimeDouble", .4, 0., 3.),
+        AlertStatus.normal, AlertSize.full,
+        Priority.LOW, None, "chimeDouble", .4, 0., 3.),
 
     "dataNeededNoEntry": Alert(
         "Comma Unavailable", 
         "Data needed for calibration. Upload drive, try again", 
-        PT.LOW, None, "chimeDouble", .4, 0., 3.),
+        AlertStatus.normal, AlertSize.full,
+        Priority.LOW, None, "chimeDouble", .4, 0., 3.),
 
     "outOfSpaceNoEntry": Alert(
         "Comma Unavailable", 
         "Out of Space", 
-        PT.LOW, None, "chimeDouble", .4, 0., 3.),
+        AlertStatus.normal, AlertSize.full,
+        Priority.LOW, None, "chimeDouble", .4, 0., 3.),
 
     "pedalPressedNoEntry": Alert(
         "Comma Unavailable", 
         "Pedal Pressed", 
-        PT.LOW, "brakePressed", "chimeDouble", .4, 2., 3.),
+        AlertStatus.normal, AlertSize.full,
+        Priority.LOW, "brakePressed", "chimeDouble", .4, 2., 3.),
 
     "speedTooLowNoEntry": Alert(
         "Comma Unavailable", 
         "Speed Too Low", 
-        PT.LOW, None, "chimeDouble", .4, 2., 3.),
+        AlertStatus.normal, AlertSize.full,
+        Priority.LOW, None, "chimeDouble", .4, 2., 3.),
 
     "brakeHoldNoEntry": Alert(
         "Comma Unavailable", 
         "Brake Hold Active", 
-        PT.LOW, None, "chimeDouble", .4, 2., 3.),
+        AlertStatus.normal, AlertSize.full,
+        Priority.LOW, None, "chimeDouble", .4, 2., 3.),
 
     "parkBrakeNoEntry": Alert(
         "Comma Unavailable", 
         "Park Brake Engaged", 
-        PT.LOW, None, "chimeDouble", .4, 2., 3.),
+        AlertStatus.normal, AlertSize.full,
+        Priority.LOW, None, "chimeDouble", .4, 2., 3.),
 
     "lowSpeedLockoutNoEntry": Alert(
         "Comma Unavailable",
         "Cruise Fault: Restart the Car",
-        PT.LOW, None, "chimeDouble", .4, 2., 3.),
+        AlertStatus.normal, AlertSize.full,
+        Priority.LOW, None, "chimeDouble", .4, 2., 3.),
 
-    # Cancellation alerts causing disabling
+    # Cancellation alerts causing soft disabling
     "overheat": Alert(
         "Take Control Immediately", 
         "System Overheated", 
-        PT.MID, "steerRequired", "chimeRepeated", 1., 3., 3.),
+        AlertStatus.critical, AlertSize.full,
+        Priority.MID, "steerRequired", "chimeRepeated", 1., 3., 3.),
 
     "wrongGear": Alert(
         "Take Control Immediately", 
         "Gear not D", 
-        PT.MID, "steerRequired", "chimeRepeated", 1., 3., 3.),
+        AlertStatus.critical, AlertSize.full,
+        Priority.MID, "steerRequired", "chimeRepeated", 1., 3., 3.),
 
     "calibrationInvalid": Alert(
         "Take Control Immediately", 
-        "Calibration Invalid: Reposition Neo and Recalibrate", 
-        PT.MID, "steerRequired", "chimeRepeated", 1., 3., 3.),
+        "Calibration Invalid: Reposition EON and Recalibrate", 
+        AlertStatus.critical, AlertSize.full,
+        Priority.MID, "steerRequired", "chimeRepeated", 1., 3., 3.),
 
     "calibrationInProgress": Alert(
         "Take Control Immediately", 
         "Calibration in Progress",
-        PT.MID, "steerRequired", "chimeRepeated", 1., 3., 3.),
+        AlertStatus.critical, AlertSize.full,
+        Priority.MID, "steerRequired", "chimeRepeated", 1., 3., 3.),
 
     "doorOpen": Alert(
         "Take Control Immediately", 
         "Door Open", 
-        PT.MID, "steerRequired", "chimeRepeated", 1., 3., 3.),
+        AlertStatus.critical, AlertSize.full,
+        Priority.MID, "steerRequired", "chimeRepeated", 1., 3., 3.),
 
     "seatbeltNotLatched": Alert(
         "Take Control Immediately", 
         "Seatbelt Unlatched", 
-        PT.MID, "steerRequired", "chimeRepeated", 1., 3., 3.),
+        AlertStatus.critical, AlertSize.full,
+        Priority.MID, "steerRequired", "chimeRepeated", 1., 3., 3.),
 
     "espDisabled": Alert(
         "Take Control Immediately", 
         "ESP Off", 
-        PT.MID, "steerRequired", "chimeRepeated", 1., 3., 3.),
+        AlertStatus.critical, AlertSize.full,
+        Priority.MID, "steerRequired", "chimeRepeated", 1., 3., 3.),
 
+    # Cancellation alerts causing immediate disabling
     "radarCommIssue": Alert(
         "Take Control Immediately", 
         "Radar Error: Restart the Car", 
-        PT.HIGH, "steerRequired", "chimeRepeated", 1., 3., 3.),
+        AlertStatus.critical, AlertSize.full,
+        Priority.HIGH, "steerRequired", "chimeRepeated", 1., 3., 4.),
 
     "radarFault": Alert(
         "Take Control Immediately", 
         "Radar Error: Restart the Car", 
-        PT.HIGH, "steerRequired", "chimeRepeated", 1., 3., 3.),
+        AlertStatus.critical, AlertSize.full,
+        Priority.HIGH, "steerRequired", "chimeRepeated", 1., 3., 4.),
 
     "modelCommIssue": Alert(
         "Take Control Immediately", 
         "Model Error: Restart the Car", 
-        PT.HIGH, "steerRequired", "chimeRepeated", 1., 3., 3.),
+        AlertStatus.critical, AlertSize.full,
+        Priority.HIGH, "steerRequired", "chimeRepeated", 1., 3., 4.),
 
     "controlsFailed": Alert(
         "Take Control Immediately", 
         "Controls Failed", 
-        PT.HIGH, "steerRequired", "chimeRepeated", 1., 3., 3.),
+        AlertStatus.critical, AlertSize.full,
+        Priority.HIGH, "steerRequired", "chimeRepeated", 1., 3., 4.),
 
     "controlsMismatch": Alert(
         "Take Control Immediately", 
         "Controls Mismatch", 
-        PT.HIGH, "steerRequired", "chimeRepeated", 1., 3., 3.),
+        AlertStatus.critical, AlertSize.full,
+        Priority.HIGH, "steerRequired", "chimeRepeated", 1., 3., 4.),
 
     "commIssue": Alert(
         "Take Control Immediately", 
         "CAN Error: Restart the Car", 
-        PT.HIGH, "steerRequired", "chimeRepeated", 1., 3., 3.),
+        AlertStatus.critical, AlertSize.full,
+        Priority.HIGH, "steerRequired", "chimeRepeated", 1., 3., 4.),
 
     "steerUnavailable": Alert(
         "Take Control Immediately", 
-        "Steer Error: Restart the Car", 
-        PT.HIGH, "steerRequired", "chimeRepeated", 1., 3., 3.),
+        "Steer Fault: Restart the Car", 
+        AlertStatus.critical, AlertSize.full,
+        Priority.HIGH, "steerRequired", "chimeRepeated", 1., 3., 4.),
 
     "brakeUnavailable": Alert(
         "Take Control Immediately", 
-        "Brake Error: Restart the Car", 
-        PT.HIGH, "steerRequired", "chimeRepeated", 1., 3., 3.),
+        "Brake Fault: Restart the Car", 
+        AlertStatus.critical, AlertSize.full,
+        Priority.HIGH, "steerRequired", "chimeRepeated", 1., 3., 4.),
 
     "gasUnavailable": Alert(
         "Take Control Immediately", 
-        "Gas Error: Restart the Car", 
-        PT.HIGH, "steerRequired", "chimeRepeated", 1., 3., 3.),
+        "Gas Fault: Restart the Car", 
+        AlertStatus.critical, AlertSize.full,
+        Priority.HIGH, "steerRequired", "chimeRepeated", 1., 3., 4.),
 
     "reverseGear": Alert(
         "Take Control Immediately", 
         "Reverse Gear", 
-        PT.HIGH, "steerRequired", "chimeRepeated", 1., 3., 3.),
+        AlertStatus.critical, AlertSize.full,
+        Priority.HIGH, "steerRequired", "chimeRepeated", 1., 3., 4.),
 
     "cruiseDisabled": Alert(
         "Take Control Immediately", 
         "Cruise Is Off", 
-        PT.HIGH, "steerRequired", "chimeRepeated", 1., 3., 3.),
+        AlertStatus.critical, AlertSize.full,
+        Priority.HIGH, "steerRequired", "chimeRepeated", 1., 3., 4.),
 
     # not loud cancellations (user is in control)
     "noTarget": Alert(
         "Comma Canceled",
         "No Close Lead", 
-        PT.HIGH, None, "chimeDouble", .4, 2., 3.),
+        AlertStatus.normal, AlertSize.full,
+        Priority.HIGH, None, "chimeDouble", .4, 2., 3.),
 
     "speedTooLow": Alert(
         "Comma Canceled",
         "Speed Too Low", 
-        PT.HIGH, None, "chimeDouble", .4, 2., 3.),
+        AlertStatus.normal, AlertSize.full,
+        Priority.HIGH, None, "chimeDouble", .4, 2., 3.),
 
     # Cancellation alerts causing non-entry
     "overheatNoEntry": Alert(
         "Comma Unavailable", 
         "System Overheated", 
-        PT.LOW, None, "chimeDouble", .4, 2., 3.),
+        AlertStatus.normal, AlertSize.full,
+        Priority.LOW, None, "chimeDouble", .4, 2., 3.),
 
     "wrongGearNoEntry": Alert(
         "Comma Unavailable", 
         "Gear not D", 
-        PT.LOW, None, "chimeDouble", .4, 2., 3.),
+        AlertStatus.normal, AlertSize.full,
+        Priority.LOW, None, "chimeDouble", .4, 2., 3.),
 
     "calibrationInvalidNoEntry": Alert(
         "Comma Unavailable", 
-        "Calibration Invalid: Reposition Neo and Recalibrate", 
-        PT.LOW, None, "chimeDouble", .4, 2., 3.),
+        "Calibration Invalid: Reposition EON and Recalibrate", 
+        AlertStatus.normal, AlertSize.full,
+        Priority.LOW, None, "chimeDouble", .4, 2., 3.),
 
     "calibrationInProgressNoEntry": Alert(
         "Comma Unavailable", 
         "Calibration in Progress", 
-        PT.LOW, None, "chimeDouble", .4, 2., 3.),
+        AlertStatus.normal, AlertSize.full,
+        Priority.LOW, None, "chimeDouble", .4, 2., 3.),
 
     "doorOpenNoEntry": Alert(
         "Comma Unavailable", 
         "Door Open", 
-        PT.LOW, None, "chimeDouble", .4, 2., 3.),
+        AlertStatus.normal, AlertSize.full,
+        Priority.LOW, None, "chimeDouble", .4, 2., 3.),
 
     "seatbeltNotLatchedNoEntry": Alert(
         "Comma Unavailable", 
         "Seatbelt Unlatched", 
-        PT.LOW, None, "chimeDouble", .4, 2., 3.),
+        AlertStatus.normal, AlertSize.full,
+        Priority.LOW, None, "chimeDouble", .4, 2., 3.),
  
     "espDisabledNoEntry": Alert(
         "Comma Unavailable", 
         "ESP Off", 
-        PT.LOW, None, "chimeDouble", .4, 2., 3.),
+        AlertStatus.normal, AlertSize.full,
+        Priority.LOW, None, "chimeDouble", .4, 2., 3.),
 
     "radarCommIssueNoEntry": Alert(
         "Comma Unavailable", 
         "Radar Error: Restart the Car", 
-        PT.LOW, None, "chimeDouble", .4, 2., 3.),
+        AlertStatus.normal, AlertSize.full,
+        Priority.LOW, None, "chimeDouble", .4, 2., 3.),
 
     "radarFaultNoEntry": Alert(
         "Comma Unavailable", 
         "Radar Error: Restart the Car", 
-        PT.LOW, None, "chimeDouble", .4, 2., 3.),
+        AlertStatus.normal, AlertSize.full,
+        Priority.LOW, None, "chimeDouble", .4, 2., 3.),
 
     "modelCommIssueNoEntry": Alert(
         "Comma Unavailable", 
         "Model Error: Restart the Car", 
-        PT.LOW, None, "chimeDouble", .4, 2., 3.),
+        AlertStatus.normal, AlertSize.full,
+        Priority.LOW, None, "chimeDouble", .4, 2., 3.),
 
     "controlsFailedNoEntry": Alert(
         "Comma Unavailable", 
         "Controls Failed", 
-        PT.LOW, None, "chimeDouble", .4, 2., 3.),
-
-    "controlsMismatchNoEntry": Alert(
-        "Comma Unavailable", 
-        "Controls Mismatch", 
-        PT.LOW, None, "chimeDouble", .4, 2., 3.),
+        AlertStatus.normal, AlertSize.full,
+        Priority.LOW, None, "chimeDouble", .4, 2., 3.),
 
     "commIssueNoEntry": Alert(
         "Comma Unavailable", 
         "CAN Error: Restart the Car", 
-        PT.LOW, None, "chimeDouble", .4, 2., 3.),
+        AlertStatus.normal, AlertSize.full,
+        Priority.LOW, None, "chimeDouble", .4, 2., 3.),
 
     "steerUnavailableNoEntry": Alert(
         "Comma Unavailable", 
-        "Steer Error: Restart the Car", 
-        PT.LOW, None, "chimeDouble", .4, 2., 3.),
+        "Steer Fault: Restart the Car", 
+        AlertStatus.normal, AlertSize.full,
+        Priority.LOW, None, "chimeDouble", .4, 2., 3.),
 
     "brakeUnavailableNoEntry": Alert(
         "Comma Unavailable", 
-        "Brake Error: Restart the Car", 
-        PT.LOW, None, "chimeDouble", .4, 2., 3.),
+        "Brake Fault: Restart the Car", 
+        AlertStatus.normal, AlertSize.full,
+        Priority.LOW, None, "chimeDouble", .4, 2., 3.),
 
     "gasUnavailableNoEntry": Alert(
         "Comma Unavailable", 
         "Gas Error: Restart the Car", 
-        PT.LOW, None, "chimeDouble", .4, 2., 3.),
+        AlertStatus.normal, AlertSize.full,
+        Priority.LOW, None, "chimeDouble", .4, 2., 3.),
 
     "reverseGearNoEntry": Alert(
         "Comma Unavailable", 
         "Reverse Gear", 
-        PT.LOW, None, "chimeDouble", .4, 2., 3.),
+        AlertStatus.normal, AlertSize.full,
+        Priority.LOW, None, "chimeDouble", .4, 2., 3.),
 
     "cruiseDisabledNoEntry": Alert(
         "Comma Unavailable", 
         "Cruise is Off", 
-        PT.LOW, None, "chimeDouble", .4, 2., 3.),
+        AlertStatus.normal, AlertSize.full,
+        Priority.LOW, None, "chimeDouble", .4, 2., 3.),
 
     "noTargetNoEntry": Alert(
         "Comma Unavailable", 
         "No Close Lead",
-        PT.LOW, None, "chimeDouble", .4, 2., 3.),
+        AlertStatus.normal, AlertSize.full,
+        Priority.LOW, None, "chimeDouble", .4, 2., 3.),
+
+    # permanent alerts to display on small UI upper box
+    "steerUnavailablePermanent": Alert(
+        "STEER FAULT", 
+        "RESTART THE CAR",
+        AlertStatus.normal, AlertSize.small,
+        Priority.LOWEST, None, None, 0., 0., .2),
+
+    "brakeUnavailablePermanent": Alert(
+        "BRAKE FAULT", 
+        "RESTART THE CAR",
+        AlertStatus.normal, AlertSize.small,
+        Priority.LOWEST, None, None, 0., 0., .2),
+
+    "lowSpeedLockoutPermanent": Alert(
+        "CRUISE FAULT", 
+        "RESTART THE CAR",
+        AlertStatus.normal, AlertSize.small,
+        Priority.LOWEST, None, None, 0., 0., .2),
   }
 
   def __init__(self):
     self.activealerts = []
-    self.current_alert = None
 
   def alertPresent(self):
     return len(self.activealerts) > 0
 
   def add(self, alert_type, enabled=True, extra_text=''):
     alert_type = str(alert_type)
-    this_alert = copy.copy(self.alerts[alert_type])
-    this_alert.alert_text_2 += extra_text
+    added_alert = copy.copy(self.alerts[alert_type])
+    added_alert.alert_text_2 += extra_text
+    added_alert.start_time = sec_since_boot()
 
     # if new alert is higher priority, log it
-    if self.current_alert is None or this_alert > self.current_alert:
+    if not self.alertPresent() or \
+       added_alert.alert_priority > self.activealerts[0].alert_priority:
       cloudlog.event('alert_add',
                      alert_type=alert_type,
                      enabled=enabled)
 
-    self.activealerts.append(this_alert)
-    self.activealerts.sort()
+    self.activealerts.append(added_alert)
+    self.activealerts.sort(key=lambda k: k.alert_priority, reverse=True)
 
+  # TODO: cycle through alerts?
   def process_alerts(self, cur_time):
-    if self.alertPresent():
-      self.alert_start_time = cur_time
-      self.current_alert = self.activealerts[0]
-      print self.current_alert
+
+    # first get rid of all the expired alerts
+    self.activealerts = [a for a in self.activealerts if a.start_time + 
+                         max(a.duration_sound, a.duration_hud_alert, a.duration_text) > cur_time]
+
+    ca = self.activealerts[0] if self.alertPresent() else None
 
     # start with assuming no alerts
     self.alert_text_1 = ""
     self.alert_text_2 = ""
+    self.alert_status = AlertStatus.normal
+    self.alert_size = AlertSize.none
     self.visual_alert = "none"
     self.audible_alert = "none"
 
-    if self.current_alert is not None:
-      # ewwwww
-      if self.alert_start_time + self.current_alert.duration_sound > cur_time:
-        self.audible_alert = self.current_alert.audible_alert
-
-      if self.alert_start_time + self.current_alert.duration_hud_alert > cur_time:
-        self.visual_alert = self.current_alert.visual_alert
+    if ca:
+      if ca.start_time + ca.duration_sound > cur_time:
+        self.audible_alert = ca.audible_alert
 
-      if self.alert_start_time + self.current_alert.duration_text > cur_time:
-        self.alert_text_1 = self.current_alert.alert_text_1
-        self.alert_text_2 = self.current_alert.alert_text_2
+      if ca.start_time + ca.duration_hud_alert > cur_time:
+        self.visual_alert = ca.visual_alert
 
-      # disable current alert
-      if self.alert_start_time + max(self.current_alert.duration_sound, self.current_alert.duration_hud_alert,
-                                     self.current_alert.duration_text) < cur_time:
-        self.current_alert = None
-
-    # reset
-    self.activealerts = []
+      if ca.start_time + ca.duration_text > cur_time:
+        self.alert_text_1 = ca.alert_text_1
+        self.alert_text_2 = ca.alert_text_2
+        self.alert_status = ca.alert_status
+        self.alert_size = ca.alert_size

selfdrive/controls/lib/drive_helpers.py

@@ -10,6 +10,7 @@ class EventTypes:
   USER_DISABLE = 'userDisable'
   SOFT_DISABLE = 'softDisable'
   IMMEDIATE_DISABLE = 'immediateDisable'
+  PERMANENT = 'permanent'
 
 
 def create_event(name, types):

selfdrive/controls/lib/latcontrol.py

@@ -8,7 +8,7 @@ from common.realtime import sec_since_boot
 from selfdrive.swaglog import cloudlog
 
 # 100ms is a rule of thumb estimation of lag from image processing to actuator command
-ACTUATORS_DELAY = 0.2
+ACTUATORS_DELAY = 0.1
 
 _DT = 0.01    # 100Hz
 _DT_MPC = 0.05  # 20Hz
@@ -28,11 +28,11 @@ class LatControl(object):
   def __init__(self, VM):
     self.pid = PIController(VM.CP.steerKp, VM.CP.steerKi, k_f=VM.CP.steerKf, pos_limit=1.0)
     self.last_cloudlog_t = 0.0
-    self.setup_mpc()
+    self.setup_mpc(VM.CP.steerRateCost)
 
-  def setup_mpc(self):
+  def setup_mpc(self, steer_rate_cost):
     self.libmpc = libmpc_py.libmpc
-    self.libmpc.init()
+    self.libmpc.init(steer_rate_cost)
 
     self.mpc_solution = libmpc_py.ffi.new("log_t *")
     self.cur_state = libmpc_py.ffi.new("state_t *")
@@ -83,7 +83,7 @@ class LatControl(object):
       nans = np.any(np.isnan(list(self.mpc_solution[0].delta)))
       t = sec_since_boot()
       if nans:
-        self.libmpc.init()
+        self.libmpc.init(VM.CP.steerRateCost)
         self.cur_state[0].delta = math.radians(angle_steers) / VM.CP.steerRatio
 
         if t > self.last_cloudlog_t + 5.0:
@@ -95,7 +95,10 @@ class LatControl(object):
       self.pid.reset()
     else:
       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)
+      # TODO: ideally we should interp, but for tuning reasons we keep the mpc solution
+      # constant for 0.05s.
+      #self.angle_steers_des = self.angle_steers_des_prev + (dt / _DT_MPC) * (self.angle_steers_des_mpc - self.angle_steers_des_prev)
+      self.angle_steers_des = self.angle_steers_des_mpc
       steers_max = get_steer_max(VM.CP, v_ego)
       self.pid.pos_limit = steers_max
       self.pid.neg_limit = -steers_max

selfdrive/controls/lib/lateral_mpc/generator.cpp

@@ -72,14 +72,12 @@ int main( )
   // Angular rate error
   h << (v_ref + 1.0 ) * t;
 
-  DMatrix Q(5,5);
-  Q(0,0) = 1.0;
-  Q(1,1) = 1.0;
-  Q(2,2) = 1.0;
-
-  Q(3,3) = 1.0;
-
-  Q(4,4) = 1.0;
+  BMatrix Q(5,5); Q.setAll(true);
+  // Q(0,0) = 1.0;
+  // Q(1,1) = 1.0;
+  // Q(2,2) = 1.0;
+  // Q(3,3) = 1.0;
+  // Q(4,4) = 2.0;
 
   // Terminal cost
   Function hN;
@@ -92,18 +90,27 @@ int main( )
   // Heading errors
   hN << (2.0 * v_ref + 1.0 ) * (angle - psi);
 
-  DMatrix QN(4,4);
-  QN(0,0) = 1.0;
-  QN(1,1) = 1.0;
-  QN(2,2) = 1.0;
-
-  QN(3,3) = 1.0;
+  BMatrix QN(4,4); QN.setAll(true);
+  // QN(0,0) = 1.0;
+  // QN(1,1) = 1.0;
+  // QN(2,2) = 1.0;
+  // QN(3,3) = 1.0;
+
+  // Non uniform time grid
+  // First 5 timesteps are 0.05, after that it's 0.15
+  DMatrix numSteps(20, 1);
+  for (int i = 0; i < 5; i++){
+    numSteps(i) = 1;
+  }
+  for (int i = 5; i < 20; i++){
+    numSteps(i) = 3;
+  }
 
   // Setup Optimal Control Problem
   const double tStart = 0.0;
-  const double tEnd   = samplingTime * controlHorizon;
+  const double tEnd   = 2.5;
 
-  OCP ocp( tStart, tEnd, controlHorizon );
+  OCP ocp( tStart, tEnd, numSteps);
   ocp.subjectTo(f);
 
   ocp.minimizeLSQ(Q, h);
@@ -120,6 +127,7 @@ int main( )
   mpc.set( INTEGRATOR_TYPE, INT_RK4 );
   mpc.set( NUM_INTEGRATOR_STEPS, 1 * controlHorizon);
   mpc.set( MAX_NUM_QP_ITERATIONS, 500);
+  mpc.set( CG_USE_VARIABLE_WEIGHTING_MATRIX, YES);
 
   mpc.set( SPARSE_QP_SOLUTION, CONDENSING );
   mpc.set( QP_SOLVER, QP_QPOASES );

selfdrive/controls/lib/lateral_mpc/libmpc_py.py

@@ -1,11 +1,12 @@
 import os
+import sys
 import subprocess
 
 from cffi import FFI
 
 mpc_dir = os.path.dirname(os.path.abspath(__file__))
 libmpc_fn = os.path.join(mpc_dir, "libcommampc.so")
-subprocess.check_output(["make", "-j4"], cwd=mpc_dir)
+subprocess.check_call(["make", "-j4"], stdout=sys.stderr, cwd=mpc_dir)
 
 ffi = FFI()
 ffi.cdef("""
@@ -14,13 +15,13 @@ typedef struct {
 } state_t;
 
 typedef struct {
-    double x[50];
-    double y[50];
-    double psi[50];
-    double delta[50];
+    double x[20];
+    double y[20];
+    double psi[20];
+    double delta[20];
 } log_t;
 
-void init();
+void init(double steer_rate_cost);
 int run_mpc(state_t * x0, log_t * solution,
              double l_poly[4], double r_poly[4], double p_poly[4],
              double l_prob, double r_prob, double p_prob, double curvature_factor, double v_ref, double lane_width);

selfdrive/controls/lib/lateral_mpc/mpc.c

@@ -28,7 +28,7 @@ typedef struct {
 	double delta[N];
 } log_t;
 
-void init(){
+void init(double steerRateCost){
   acado_initializeSolver();
   int    i;
 
@@ -42,6 +42,22 @@ void init(){
 
   /* MPC: initialize the current state feedback. */
   for (i = 0; i < NX; ++i) acadoVariables.x0[ i ] = 0.0;
+
+  for (i = 0; i < N; i++) {
+    int f = 1;
+    if (i > 4){
+      f = 3;
+    }
+    acadoVariables.W[25 * i + 0] = 1.0 * f;
+    acadoVariables.W[25 * i + 6] = 1.0 * f;
+    acadoVariables.W[25 * i + 12] = 1.0 * f;
+    acadoVariables.W[25 * i + 18] = 1.0 * f;
+    acadoVariables.W[25 * i + 24] = steerRateCost * f;
+  }
+  acadoVariables.WN[0] = 1.0;
+  acadoVariables.WN[5] = 1.0;
+  acadoVariables.WN[10] = 1.0;
+  acadoVariables.WN[15] = 1.0;
 }
 
 int run_mpc(state_t * x0, log_t * solution,

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

@@ -62,7 +62,7 @@ extern "C"
 /** Indicator for fixed initial state. */
 #define ACADO_INITIAL_STATE_FIXED 1
 /** Number of control/estimation intervals. */
-#define ACADO_N 50
+#define ACADO_N 20
 /** Number of online data values. */
 #define ACADO_NOD 18
 /** Number of path constraints. */
@@ -80,9 +80,7 @@ extern "C"
 /** Number of references/measurements on the last (N + 1)st node. */
 #define ACADO_NYN 4
 /** Total number of QP optimization variables. */
-#define ACADO_QP_NV 54
-/** Number of integration steps per shooting interval. */
-#define ACADO_RK_NIS 1
+#define ACADO_QP_NV 24
 /** Number of Runge-Kutta stages per integration step. */
 #define ACADO_RK_NSTAGES 4
 /** Providing interface for arrival cost. */
@@ -90,7 +88,7 @@ extern "C"
 /** 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
+#define ACADO_WEIGHTING_MATRICES_TYPE 2
 
 
 /*
@@ -104,36 +102,42 @@ extern "C"
 typedef struct ACADOvariables_
 {
 int dummy;
-/** Matrix of size: 51 x 4 (row major format)
+/** Matrix of size: 21 x 4 (row major format)
  * 
- *  Matrix containing 51 differential variable vectors.
+ *  Matrix containing 21 differential variable vectors.
  */
-real_t x[ 204 ];
+real_t x[ 84 ];
 
-/** Column vector of size: 50
+/** Column vector of size: 20
  * 
- *  Matrix containing 50 control variable vectors.
+ *  Matrix containing 20 control variable vectors.
  */
-real_t u[ 50 ];
+real_t u[ 20 ];
 
-/** Matrix of size: 51 x 18 (row major format)
+/** Matrix of size: 21 x 18 (row major format)
  * 
- *  Matrix containing 51 online data vectors.
+ *  Matrix containing 21 online data vectors.
  */
-real_t od[ 918 ];
+real_t od[ 378 ];
 
-/** Column vector of size: 250
+/** Column vector of size: 100
  * 
- *  Matrix containing 50 reference/measurement vectors of size 5 for first 50 nodes.
+ *  Matrix containing 20 reference/measurement vectors of size 5 for first 20 nodes.
  */
-real_t y[ 250 ];
+real_t y[ 100 ];
 
 /** Column vector of size: 4
  * 
- *  Reference/measurement vector for the 51. node.
+ *  Reference/measurement vector for the 21. node.
  */
 real_t yN[ 4 ];
 
+/** Matrix of size: 100 x 5 (row major format) */
+real_t W[ 500 ];
+
+/** Matrix of size: 4 x 4 (row major format) */
+real_t WN[ 16 ];
+
 /** Column vector of size: 4
  * 
  *  Current state feedback vector.
@@ -165,20 +169,20 @@ real_t rk_kkk[ 96 ];
 /** Row vector of size: 43 */
 real_t state[ 43 ];
 
-/** Column vector of size: 200 */
-real_t d[ 200 ];
+/** Column vector of size: 80 */
+real_t d[ 80 ];
 
-/** Column vector of size: 250 */
-real_t Dy[ 250 ];
+/** Column vector of size: 100 */
+real_t Dy[ 100 ];
 
 /** Column vector of size: 4 */
 real_t DyN[ 4 ];
 
-/** Matrix of size: 200 x 4 (row major format) */
-real_t evGx[ 800 ];
+/** Matrix of size: 80 x 4 (row major format) */
+real_t evGx[ 320 ];
 
-/** Column vector of size: 200 */
-real_t evGu[ 200 ];
+/** Column vector of size: 80 */
+real_t evGu[ 80 ];
 
 /** Column vector of size: 19 */
 real_t objAuxVar[ 19 ];
@@ -189,17 +193,20 @@ real_t objValueIn[ 23 ];
 /** Row vector of size: 30 */
 real_t objValueOut[ 30 ];
 
-/** Matrix of size: 200 x 4 (row major format) */
-real_t Q1[ 800 ];
+/** Matrix of size: 80 x 4 (row major format) */
+real_t Q1[ 320 ];
 
-/** Matrix of size: 200 x 5 (row major format) */
-real_t Q2[ 1000 ];
+/** Matrix of size: 80 x 5 (row major format) */
+real_t Q2[ 400 ];
 
-/** Column vector of size: 50 */
-real_t R1[ 50 ];
+/** Column vector of size: 20 */
+real_t R1[ 20 ];
 
-/** Matrix of size: 50 x 5 (row major format) */
-real_t R2[ 250 ];
+/** Matrix of size: 20 x 5 (row major format) */
+real_t R2[ 100 ];
+
+/** Column vector of size: 80 */
+real_t S1[ 80 ];
 
 /** Matrix of size: 4 x 4 (row major format) */
 real_t QN1[ 16 ];
@@ -213,50 +220,50 @@ real_t Dx0[ 4 ];
 /** Matrix of size: 4 x 4 (row major format) */
 real_t T[ 16 ];
 
-/** Column vector of size: 5100 */
-real_t E[ 5100 ];
+/** Column vector of size: 840 */
+real_t E[ 840 ];
 
-/** Column vector of size: 5100 */
-real_t QE[ 5100 ];
+/** Column vector of size: 840 */
+real_t QE[ 840 ];
 
-/** Matrix of size: 200 x 4 (row major format) */
-real_t QGx[ 800 ];
+/** Matrix of size: 80 x 4 (row major format) */
+real_t QGx[ 320 ];
 
-/** Column vector of size: 200 */
-real_t Qd[ 200 ];
+/** Column vector of size: 80 */
+real_t Qd[ 80 ];
 
-/** Column vector of size: 204 */
-real_t QDy[ 204 ];
+/** Column vector of size: 84 */
+real_t QDy[ 84 ];
 
-/** Matrix of size: 50 x 4 (row major format) */
-real_t H10[ 200 ];
+/** Matrix of size: 20 x 4 (row major format) */
+real_t H10[ 80 ];
 
-/** Matrix of size: 54 x 54 (row major format) */
-real_t H[ 2916 ];
+/** Matrix of size: 24 x 24 (row major format) */
+real_t H[ 576 ];
 
-/** Matrix of size: 100 x 54 (row major format) */
-real_t A[ 5400 ];
+/** Matrix of size: 40 x 24 (row major format) */
+real_t A[ 960 ];
 
-/** Column vector of size: 54 */
-real_t g[ 54 ];
+/** Column vector of size: 24 */
+real_t g[ 24 ];
 
-/** Column vector of size: 54 */
-real_t lb[ 54 ];
+/** Column vector of size: 24 */
+real_t lb[ 24 ];
 
-/** Column vector of size: 54 */
-real_t ub[ 54 ];
+/** Column vector of size: 24 */
+real_t ub[ 24 ];
 
-/** Column vector of size: 100 */
-real_t lbA[ 100 ];
+/** Column vector of size: 40 */
+real_t lbA[ 40 ];
 
-/** Column vector of size: 100 */
-real_t ubA[ 100 ];
+/** Column vector of size: 40 */
+real_t ubA[ 40 ];
 
-/** Column vector of size: 54 */
-real_t x[ 54 ];
+/** Column vector of size: 24 */
+real_t x[ 24 ];
 
-/** Column vector of size: 154 */
-real_t y[ 154 ];
+/** Column vector of size: 64 */
+real_t y[ 64 ];
 
 
 } ACADOworkspace;
@@ -270,10 +277,11 @@ real_t y[ 154 ];
  *
  *  \param rk_eta Working array to pass the input values and return the results.
  *  \param resetIntegrator The internal memory of the integrator can be reset.
+ *  \param rk_index Number of the shooting interval.
  *
  *  \return Status code of the integrator.
  */
-int acado_integrate( real_t* const rk_eta, int resetIntegrator );
+int acado_integrate( real_t* const rk_eta, int resetIntegrator, int rk_index );
 
 /** Export of an ACADO symbolic function.
  *
@@ -306,7 +314,7 @@ 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 strategy Shifting strategy: 1. Initialize node 21 with xEnd. 2. Initialize node 21 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.
  */

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

@@ -72,11 +72,13 @@ out[23] = (real_t)(1.0000000000000000e+00);
 }
 
 /* Fixed step size:0.05 */
-int acado_integrate( real_t* const rk_eta, int resetIntegrator )
+int acado_integrate( real_t* const rk_eta, int resetIntegrator, int rk_index )
 {
 int error;
 
 int run1;
+int numSteps[20] = {1, 1, 1, 1, 1, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3};
+int numInts = numSteps[rk_index];
 acadoWorkspace.rk_ttt = 0.0000000000000000e+00;
 rk_eta[4] = 1.0000000000000000e+00;
 rk_eta[5] = 0.0000000000000000e+00;
@@ -120,6 +122,7 @@ acadoWorkspace.rk_xxx[42] = rk_eta[42];
 
 for (run1 = 0; run1 < 1; ++run1)
 {
+for(run1 = 0; run1 < numInts; run1++ ) {
 acadoWorkspace.rk_xxx[0] = + rk_eta[0];
 acadoWorkspace.rk_xxx[1] = + rk_eta[1];
 acadoWorkspace.rk_xxx[2] = + rk_eta[2];
@@ -145,107 +148,108 @@ acadoWorkspace.rk_xxx[21] = + rk_eta[21];
 acadoWorkspace.rk_xxx[22] = + rk_eta[22];
 acadoWorkspace.rk_xxx[23] = + rk_eta[23];
 acado_rhs_forw( acadoWorkspace.rk_xxx, acadoWorkspace.rk_kkk );
-acadoWorkspace.rk_xxx[0] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[0] + rk_eta[0];
-acadoWorkspace.rk_xxx[1] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[1] + rk_eta[1];
-acadoWorkspace.rk_xxx[2] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[2] + rk_eta[2];
-acadoWorkspace.rk_xxx[3] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[3] + rk_eta[3];
-acadoWorkspace.rk_xxx[4] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[4] + rk_eta[4];
-acadoWorkspace.rk_xxx[5] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[5] + rk_eta[5];
-acadoWorkspace.rk_xxx[6] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[6] + rk_eta[6];
-acadoWorkspace.rk_xxx[7] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[7] + rk_eta[7];
-acadoWorkspace.rk_xxx[8] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[8] + rk_eta[8];
-acadoWorkspace.rk_xxx[9] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[9] + rk_eta[9];
-acadoWorkspace.rk_xxx[10] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[10] + rk_eta[10];
-acadoWorkspace.rk_xxx[11] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[11] + rk_eta[11];
-acadoWorkspace.rk_xxx[12] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[12] + rk_eta[12];
-acadoWorkspace.rk_xxx[13] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[13] + rk_eta[13];
-acadoWorkspace.rk_xxx[14] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[14] + rk_eta[14];
-acadoWorkspace.rk_xxx[15] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[15] + rk_eta[15];
-acadoWorkspace.rk_xxx[16] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[16] + rk_eta[16];
-acadoWorkspace.rk_xxx[17] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[17] + rk_eta[17];
-acadoWorkspace.rk_xxx[18] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[18] + rk_eta[18];
-acadoWorkspace.rk_xxx[19] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[19] + rk_eta[19];
-acadoWorkspace.rk_xxx[20] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[20] + rk_eta[20];
-acadoWorkspace.rk_xxx[21] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[21] + rk_eta[21];
-acadoWorkspace.rk_xxx[22] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[22] + rk_eta[22];
-acadoWorkspace.rk_xxx[23] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[23] + rk_eta[23];
+acadoWorkspace.rk_xxx[0] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[0] + rk_eta[0];
+acadoWorkspace.rk_xxx[1] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[1] + rk_eta[1];
+acadoWorkspace.rk_xxx[2] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[2] + rk_eta[2];
+acadoWorkspace.rk_xxx[3] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[3] + rk_eta[3];
+acadoWorkspace.rk_xxx[4] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[4] + rk_eta[4];
+acadoWorkspace.rk_xxx[5] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[5] + rk_eta[5];
+acadoWorkspace.rk_xxx[6] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[6] + rk_eta[6];
+acadoWorkspace.rk_xxx[7] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[7] + rk_eta[7];
+acadoWorkspace.rk_xxx[8] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[8] + rk_eta[8];
+acadoWorkspace.rk_xxx[9] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[9] + rk_eta[9];
+acadoWorkspace.rk_xxx[10] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[10] + rk_eta[10];
+acadoWorkspace.rk_xxx[11] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[11] + rk_eta[11];
+acadoWorkspace.rk_xxx[12] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[12] + rk_eta[12];
+acadoWorkspace.rk_xxx[13] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[13] + rk_eta[13];
+acadoWorkspace.rk_xxx[14] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[14] + rk_eta[14];
+acadoWorkspace.rk_xxx[15] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[15] + rk_eta[15];
+acadoWorkspace.rk_xxx[16] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[16] + rk_eta[16];
+acadoWorkspace.rk_xxx[17] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[17] + rk_eta[17];
+acadoWorkspace.rk_xxx[18] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[18] + rk_eta[18];
+acadoWorkspace.rk_xxx[19] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[19] + rk_eta[19];
+acadoWorkspace.rk_xxx[20] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[20] + rk_eta[20];
+acadoWorkspace.rk_xxx[21] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[21] + rk_eta[21];
+acadoWorkspace.rk_xxx[22] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[22] + rk_eta[22];
+acadoWorkspace.rk_xxx[23] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[23] + rk_eta[23];
 acado_rhs_forw( acadoWorkspace.rk_xxx, &(acadoWorkspace.rk_kkk[ 24 ]) );
-acadoWorkspace.rk_xxx[0] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[24] + rk_eta[0];
-acadoWorkspace.rk_xxx[1] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[25] + rk_eta[1];
-acadoWorkspace.rk_xxx[2] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[26] + rk_eta[2];
-acadoWorkspace.rk_xxx[3] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[27] + rk_eta[3];
-acadoWorkspace.rk_xxx[4] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[28] + rk_eta[4];
-acadoWorkspace.rk_xxx[5] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[29] + rk_eta[5];
-acadoWorkspace.rk_xxx[6] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[30] + rk_eta[6];
-acadoWorkspace.rk_xxx[7] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[31] + rk_eta[7];
-acadoWorkspace.rk_xxx[8] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[32] + rk_eta[8];
-acadoWorkspace.rk_xxx[9] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[33] + rk_eta[9];
-acadoWorkspace.rk_xxx[10] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[34] + rk_eta[10];
-acadoWorkspace.rk_xxx[11] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[35] + rk_eta[11];
-acadoWorkspace.rk_xxx[12] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[36] + rk_eta[12];
-acadoWorkspace.rk_xxx[13] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[37] + rk_eta[13];
-acadoWorkspace.rk_xxx[14] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[38] + rk_eta[14];
-acadoWorkspace.rk_xxx[15] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[39] + rk_eta[15];
-acadoWorkspace.rk_xxx[16] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[40] + rk_eta[16];
-acadoWorkspace.rk_xxx[17] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[41] + rk_eta[17];
-acadoWorkspace.rk_xxx[18] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[42] + rk_eta[18];
-acadoWorkspace.rk_xxx[19] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[43] + rk_eta[19];
-acadoWorkspace.rk_xxx[20] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[44] + rk_eta[20];
-acadoWorkspace.rk_xxx[21] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[45] + rk_eta[21];
-acadoWorkspace.rk_xxx[22] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[46] + rk_eta[22];
-acadoWorkspace.rk_xxx[23] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[47] + rk_eta[23];
+acadoWorkspace.rk_xxx[0] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[24] + rk_eta[0];
+acadoWorkspace.rk_xxx[1] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[25] + rk_eta[1];
+acadoWorkspace.rk_xxx[2] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[26] + rk_eta[2];
+acadoWorkspace.rk_xxx[3] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[27] + rk_eta[3];
+acadoWorkspace.rk_xxx[4] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[28] + rk_eta[4];
+acadoWorkspace.rk_xxx[5] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[29] + rk_eta[5];
+acadoWorkspace.rk_xxx[6] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[30] + rk_eta[6];
+acadoWorkspace.rk_xxx[7] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[31] + rk_eta[7];
+acadoWorkspace.rk_xxx[8] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[32] + rk_eta[8];
+acadoWorkspace.rk_xxx[9] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[33] + rk_eta[9];
+acadoWorkspace.rk_xxx[10] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[34] + rk_eta[10];
+acadoWorkspace.rk_xxx[11] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[35] + rk_eta[11];
+acadoWorkspace.rk_xxx[12] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[36] + rk_eta[12];
+acadoWorkspace.rk_xxx[13] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[37] + rk_eta[13];
+acadoWorkspace.rk_xxx[14] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[38] + rk_eta[14];
+acadoWorkspace.rk_xxx[15] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[39] + rk_eta[15];
+acadoWorkspace.rk_xxx[16] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[40] + rk_eta[16];
+acadoWorkspace.rk_xxx[17] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[41] + rk_eta[17];
+acadoWorkspace.rk_xxx[18] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[42] + rk_eta[18];
+acadoWorkspace.rk_xxx[19] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[43] + rk_eta[19];
+acadoWorkspace.rk_xxx[20] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[44] + rk_eta[20];
+acadoWorkspace.rk_xxx[21] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[45] + rk_eta[21];
+acadoWorkspace.rk_xxx[22] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[46] + rk_eta[22];
+acadoWorkspace.rk_xxx[23] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[47] + rk_eta[23];
 acado_rhs_forw( acadoWorkspace.rk_xxx, &(acadoWorkspace.rk_kkk[ 48 ]) );
-acadoWorkspace.rk_xxx[0] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[48] + rk_eta[0];
-acadoWorkspace.rk_xxx[1] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[49] + rk_eta[1];
-acadoWorkspace.rk_xxx[2] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[50] + rk_eta[2];
-acadoWorkspace.rk_xxx[3] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[51] + rk_eta[3];
-acadoWorkspace.rk_xxx[4] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[52] + rk_eta[4];
-acadoWorkspace.rk_xxx[5] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[53] + rk_eta[5];
-acadoWorkspace.rk_xxx[6] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[54] + rk_eta[6];
-acadoWorkspace.rk_xxx[7] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[55] + rk_eta[7];
-acadoWorkspace.rk_xxx[8] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[56] + rk_eta[8];
-acadoWorkspace.rk_xxx[9] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[57] + rk_eta[9];
-acadoWorkspace.rk_xxx[10] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[58] + rk_eta[10];
-acadoWorkspace.rk_xxx[11] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[59] + rk_eta[11];
-acadoWorkspace.rk_xxx[12] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[60] + rk_eta[12];
-acadoWorkspace.rk_xxx[13] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[61] + rk_eta[13];
-acadoWorkspace.rk_xxx[14] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[62] + rk_eta[14];
-acadoWorkspace.rk_xxx[15] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[63] + rk_eta[15];
-acadoWorkspace.rk_xxx[16] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[64] + rk_eta[16];
-acadoWorkspace.rk_xxx[17] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[65] + rk_eta[17];
-acadoWorkspace.rk_xxx[18] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[66] + rk_eta[18];
-acadoWorkspace.rk_xxx[19] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[67] + rk_eta[19];
-acadoWorkspace.rk_xxx[20] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[68] + rk_eta[20];
-acadoWorkspace.rk_xxx[21] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[69] + rk_eta[21];
-acadoWorkspace.rk_xxx[22] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[70] + rk_eta[22];
-acadoWorkspace.rk_xxx[23] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[71] + rk_eta[23];
+acadoWorkspace.rk_xxx[0] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[48] + rk_eta[0];
+acadoWorkspace.rk_xxx[1] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[49] + rk_eta[1];
+acadoWorkspace.rk_xxx[2] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[50] + rk_eta[2];
+acadoWorkspace.rk_xxx[3] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[51] + rk_eta[3];
+acadoWorkspace.rk_xxx[4] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[52] + rk_eta[4];
+acadoWorkspace.rk_xxx[5] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[53] + rk_eta[5];
+acadoWorkspace.rk_xxx[6] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[54] + rk_eta[6];
+acadoWorkspace.rk_xxx[7] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[55] + rk_eta[7];
+acadoWorkspace.rk_xxx[8] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[56] + rk_eta[8];
+acadoWorkspace.rk_xxx[9] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[57] + rk_eta[9];
+acadoWorkspace.rk_xxx[10] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[58] + rk_eta[10];
+acadoWorkspace.rk_xxx[11] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[59] + rk_eta[11];
+acadoWorkspace.rk_xxx[12] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[60] + rk_eta[12];
+acadoWorkspace.rk_xxx[13] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[61] + rk_eta[13];
+acadoWorkspace.rk_xxx[14] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[62] + rk_eta[14];
+acadoWorkspace.rk_xxx[15] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[63] + rk_eta[15];
+acadoWorkspace.rk_xxx[16] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[64] + rk_eta[16];
+acadoWorkspace.rk_xxx[17] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[65] + rk_eta[17];
+acadoWorkspace.rk_xxx[18] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[66] + rk_eta[18];
+acadoWorkspace.rk_xxx[19] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[67] + rk_eta[19];
+acadoWorkspace.rk_xxx[20] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[68] + rk_eta[20];
+acadoWorkspace.rk_xxx[21] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[69] + rk_eta[21];
+acadoWorkspace.rk_xxx[22] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[70] + rk_eta[22];
+acadoWorkspace.rk_xxx[23] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[71] + rk_eta[23];
 acado_rhs_forw( acadoWorkspace.rk_xxx, &(acadoWorkspace.rk_kkk[ 72 ]) );
-rk_eta[0] += + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[0] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[24] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[48] + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[72];
-rk_eta[1] += + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[1] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[25] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[49] + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[73];
-rk_eta[2] += + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[2] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[26] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[50] + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[74];
-rk_eta[3] += + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[3] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[27] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[51] + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[75];
-rk_eta[4] += + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[4] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[28] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[52] + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[76];
-rk_eta[5] += + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[5] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[29] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[53] + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[77];
-rk_eta[6] += + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[6] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[30] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[54] + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[78];
-rk_eta[7] += + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[7] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[31] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[55] + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[79];
-rk_eta[8] += + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[8] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[32] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[56] + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[80];
-rk_eta[9] += + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[9] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[33] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[57] + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[81];
-rk_eta[10] += + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[10] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[34] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[58] + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[82];
-rk_eta[11] += + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[11] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[35] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[59] + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[83];
-rk_eta[12] += + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[12] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[36] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[60] + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[84];
-rk_eta[13] += + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[13] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[37] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[61] + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[85];
-rk_eta[14] += + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[14] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[38] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[62] + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[86];
-rk_eta[15] += + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[15] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[39] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[63] + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[87];
-rk_eta[16] += + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[16] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[40] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[64] + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[88];
-rk_eta[17] += + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[17] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[41] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[65] + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[89];
-rk_eta[18] += + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[18] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[42] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[66] + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[90];
-rk_eta[19] += + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[19] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[43] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[67] + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[91];
-rk_eta[20] += + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[20] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[44] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[68] + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[92];
-rk_eta[21] += + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[21] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[45] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[69] + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[93];
-rk_eta[22] += + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[22] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[46] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[70] + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[94];
-rk_eta[23] += + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[23] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[47] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[71] + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[95];
+rk_eta[0] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[0] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[24] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[48] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[72];
+rk_eta[1] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[1] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[25] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[49] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[73];
+rk_eta[2] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[2] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[26] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[50] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[74];
+rk_eta[3] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[3] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[27] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[51] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[75];
+rk_eta[4] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[4] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[28] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[52] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[76];
+rk_eta[5] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[5] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[29] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[53] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[77];
+rk_eta[6] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[6] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[30] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[54] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[78];
+rk_eta[7] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[7] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[31] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[55] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[79];
+rk_eta[8] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[8] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[32] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[56] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[80];
+rk_eta[9] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[9] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[33] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[57] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[81];
+rk_eta[10] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[10] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[34] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[58] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[82];
+rk_eta[11] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[11] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[35] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[59] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[83];
+rk_eta[12] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[12] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[36] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[60] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[84];
+rk_eta[13] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[13] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[37] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[61] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[85];
+rk_eta[14] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[14] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[38] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[62] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[86];
+rk_eta[15] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[15] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[39] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[63] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[87];
+rk_eta[16] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[16] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[40] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[64] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[88];
+rk_eta[17] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[17] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[41] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[65] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[89];
+rk_eta[18] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[18] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[42] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[66] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[90];
+rk_eta[19] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[19] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[43] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[67] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[91];
+rk_eta[20] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[20] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[44] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[68] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[92];
+rk_eta[21] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[21] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[45] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[69] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[93];
+rk_eta[22] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[22] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[46] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[70] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[94];
+rk_eta[23] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[23] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[47] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[71] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[95];
 acadoWorkspace.rk_ttt += 1.0000000000000000e+00;
 }
+}
 error = 0;
 return error;
 }

selfdrive/controls/lib/lateral_mpc/mpc_export/acado_qpoases_interface.cpp

@@ -40,7 +40,7 @@ int acado_solve( void )
 {
 	acado_nWSR = QPOASES_NWSRMAX;
 
-	QProblem qp(54, 100);
+	QProblem qp(24, 40);
 	
 	returnValue retVal = qp.init(acadoWorkspace.H, acadoWorkspace.g, acadoWorkspace.A, acadoWorkspace.lb, acadoWorkspace.ub, acadoWorkspace.lbA, acadoWorkspace.ubA, acado_nWSR, acadoWorkspace.y);
 

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

@@ -37,9 +37,9 @@
  */
 
 /** Maximum number of optimization variables. */
-#define QPOASES_NVMAX      54
+#define QPOASES_NVMAX      24
 /** Maximum number of constraints. */
-#define QPOASES_NCMAX      100
+#define QPOASES_NCMAX      40
 /** Maximum number of working set recalculations. */
 #define QPOASES_NWSRMAX    500
 /** Print level for qpOASES. */

selfdrive/controls/lib/longcontrol.py

@@ -4,7 +4,7 @@ from selfdrive.config import Conversions as CV
 from selfdrive.controls.lib.pid import PIController
 
 STOPPING_EGO_SPEED = 0.5
-MIN_CAN_SPEED = 0.3
+MIN_CAN_SPEED = 0.3  #TODO: parametrize this in car interface
 STOPPING_TARGET_SPEED = MIN_CAN_SPEED + 0.01
 STARTING_TARGET_SPEED = 0.5
 BRAKE_THRESHOLD_TO_PID = 0.2
@@ -101,7 +101,7 @@ class LongControl(object):
     elif self.long_control_state == LongCtrlState.pid:
       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.v_pid = v_target
       self.pid.pos_limit = gas_max
       self.pid.neg_limit = - brake_max
       deadzone = interp(v_ego_pid, CP.longPidDeadzoneBP, CP.longPidDeadzoneV)

selfdrive/controls/lib/longitudinal_mpc/generator.cpp

@@ -44,11 +44,8 @@ int main( )
   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;
+  // Weights are defined in mpc.
+  BMatrix Q(4,4); Q.setAll(true);
 
   // Terminal cost
   Function hN;
@@ -56,16 +53,24 @@ int main( )
   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;
+  // Weights are defined in mpc.
+  BMatrix QN(3,3); QN.setAll(true);
+
+  // Non uniform time grid
+  // First 5 timesteps are 0.2, after that it's 0.6
+  DMatrix numSteps(20, 1);
+  for (int i = 0; i < 5; i++){
+    numSteps(i) = 1;
+  }
+  for (int i = 5; i < 20; i++){
+    numSteps(i) = 3;
+  }
 
   // Setup Optimal Control Problem
   const double tStart = 0.0;
-  const double tEnd   = samplingTime * controlHorizon;
+  const double tEnd   = 10.0;
 
-  OCP ocp( tStart, tEnd, controlHorizon );
+  OCP ocp( tStart, tEnd, numSteps);
   ocp.subjectTo(f);
 
   ocp.minimizeLSQ(Q, h);
@@ -78,8 +83,9 @@ int main( )
   mpc.set( HESSIAN_APPROXIMATION, GAUSS_NEWTON );
   mpc.set( DISCRETIZATION_TYPE, MULTIPLE_SHOOTING );
   mpc.set( INTEGRATOR_TYPE, INT_RK4 );
-  mpc.set( NUM_INTEGRATOR_STEPS, 1 * controlHorizon);
+  mpc.set( NUM_INTEGRATOR_STEPS, controlHorizon);
   mpc.set( MAX_NUM_QP_ITERATIONS, 500);
+  mpc.set( CG_USE_VARIABLE_WEIGHTING_MATRIX, YES);
 
   mpc.set( SPARSE_QP_SOLUTION, CONDENSING );
   mpc.set( QP_SOLVER, QP_QPOASES );

selfdrive/controls/lib/longitudinal_mpc/libmpc_py.py

@@ -1,10 +1,11 @@
 import os
+import sys
 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)
+subprocess.check_call(["make", "-j4"], stdout=sys.stderr, cwd=mpc_dir)
 
 
 def _get_libmpc(mpc_id):
@@ -18,13 +19,13 @@ def _get_libmpc(mpc_id):
 
 
     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];
+    double x_ego[20];
+    double v_ego[20];
+    double a_ego[20];
+    double j_ego[20];
+    double x_l[20];
+    double v_l[20];
+    double a_l[20];
     } log_t;
 
     void init();

selfdrive/controls/lib/longitudinal_mpc/mpc.c

@@ -45,6 +45,22 @@ void init(){
 
   /* MPC: initialize the current state feedback. */
   for (i = 0; i < NX; ++i) acadoVariables.x0[ i ] = 0.0;
+  // Set weights
+
+  for (i = 0; i < N; i++) {
+    int f = 1;
+    if (i > 4){
+      f = 3;
+    }
+    acadoVariables.W[16 * i + 0] = 5.0 * f; // exponential cost for danger zone
+    acadoVariables.W[16 * i + 5] = 0.1 * f; // desired distance
+    acadoVariables.W[16 * i + 10] = 10.0 * f; // acceleration
+    acadoVariables.W[16 * i + 15] = 20.0 * f; // jerk
+  }
+  acadoVariables.WN[0] = 5.0; // exponential cost for danger zone
+  acadoVariables.WN[4] = 0.1; // desired distance
+  acadoVariables.WN[8] = 10.0; // acceleration
+
 }
 
 void init_with_simulation(double v_ego, double x_l, double v_l, double a_l, double l){
@@ -58,6 +74,9 @@ void init_with_simulation(double v_ego, double x_l, double v_l, double a_l, doub
   double dt = 0.2;
 
   for (i = 0; i < N + 1; ++i){
+    if (i > 4){
+      dt = 0.6;
+    }
     acadoVariables.x[i*NX] = x_ego;
     acadoVariables.x[i*NX+1] = v_ego;
     acadoVariables.x[i*NX+2] = a_ego;

selfdrive/controls/lib/longitudinal_mpc/mpc_export/acado_common.h

@@ -62,7 +62,7 @@ extern "C"
 /** Indicator for fixed initial state. */
 #define ACADO_INITIAL_STATE_FIXED 1
 /** Number of control/estimation intervals. */
-#define ACADO_N 50
+#define ACADO_N 20
 /** Number of online data values. */
 #define ACADO_NOD 1
 /** Number of path constraints. */
@@ -80,9 +80,7 @@ extern "C"
 /** 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
+#define ACADO_QP_NV 26
 /** Number of Runge-Kutta stages per integration step. */
 #define ACADO_RK_NSTAGES 4
 /** Providing interface for arrival cost. */
@@ -90,7 +88,7 @@ extern "C"
 /** 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
+#define ACADO_WEIGHTING_MATRICES_TYPE 2
 
 
 /*
@@ -104,36 +102,42 @@ extern "C"
 typedef struct ACADOvariables_
 {
 int dummy;
-/** Matrix of size: 51 x 6 (row major format)
+/** Matrix of size: 21 x 6 (row major format)
  * 
- *  Matrix containing 51 differential variable vectors.
+ *  Matrix containing 21 differential variable vectors.
  */
-real_t x[ 306 ];
+real_t x[ 126 ];
 
-/** Column vector of size: 50
+/** Column vector of size: 20
  * 
- *  Matrix containing 50 control variable vectors.
+ *  Matrix containing 20 control variable vectors.
  */
-real_t u[ 50 ];
+real_t u[ 20 ];
 
-/** Column vector of size: 51
+/** Column vector of size: 21
  * 
- *  Matrix containing 51 online data vectors.
+ *  Matrix containing 21 online data vectors.
  */
-real_t od[ 51 ];
+real_t od[ 21 ];
 
-/** Column vector of size: 200
+/** Column vector of size: 80
  * 
- *  Matrix containing 50 reference/measurement vectors of size 4 for first 50 nodes.
+ *  Matrix containing 20 reference/measurement vectors of size 4 for first 20 nodes.
  */
-real_t y[ 200 ];
+real_t y[ 80 ];
 
 /** Column vector of size: 3
  * 
- *  Reference/measurement vector for the 51. node.
+ *  Reference/measurement vector for the 21. node.
  */
 real_t yN[ 3 ];
 
+/** Matrix of size: 80 x 4 (row major format) */
+real_t W[ 320 ];
+
+/** Matrix of size: 3 x 3 (row major format) */
+real_t WN[ 9 ];
+
 /** Column vector of size: 6
  * 
  *  Current state feedback vector.
@@ -162,20 +166,20 @@ 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: 120 */
+real_t d[ 120 ];
 
-/** Column vector of size: 200 */
-real_t Dy[ 200 ];
+/** Column vector of size: 80 */
+real_t Dy[ 80 ];
 
 /** Column vector of size: 3 */
 real_t DyN[ 3 ];
 
-/** Matrix of size: 300 x 6 (row major format) */
-real_t evGx[ 1800 ];
+/** Matrix of size: 120 x 6 (row major format) */
+real_t evGx[ 720 ];
 
-/** Column vector of size: 300 */
-real_t evGu[ 300 ];
+/** Column vector of size: 120 */
+real_t evGu[ 120 ];
 
 /** Column vector of size: 32 */
 real_t objAuxVar[ 32 ];
@@ -186,20 +190,20 @@ 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: 120 x 6 (row major format) */
+real_t Q1[ 720 ];
 
-/** Matrix of size: 300 x 4 (row major format) */
-real_t Q2[ 1200 ];
+/** Matrix of size: 120 x 4 (row major format) */
+real_t Q2[ 480 ];
 
-/** Column vector of size: 50 */
-real_t R1[ 50 ];
+/** Column vector of size: 20 */
+real_t R1[ 20 ];
 
-/** Matrix of size: 50 x 4 (row major format) */
-real_t R2[ 200 ];
+/** Matrix of size: 20 x 4 (row major format) */
+real_t R2[ 80 ];
 
-/** Column vector of size: 300 */
-real_t S1[ 300 ];
+/** Column vector of size: 120 */
+real_t S1[ 120 ];
 
 /** Matrix of size: 6 x 6 (row major format) */
 real_t QN1[ 36 ];
@@ -213,50 +217,50 @@ 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: 1260 */
+real_t E[ 1260 ];
 
-/** Column vector of size: 7650 */
-real_t QE[ 7650 ];
+/** Column vector of size: 1260 */
+real_t QE[ 1260 ];
 
-/** Matrix of size: 300 x 6 (row major format) */
-real_t QGx[ 1800 ];
+/** Matrix of size: 120 x 6 (row major format) */
+real_t QGx[ 720 ];
 
-/** Column vector of size: 300 */
-real_t Qd[ 300 ];
+/** Column vector of size: 120 */
+real_t Qd[ 120 ];
 
-/** Column vector of size: 306 */
-real_t QDy[ 306 ];
+/** Column vector of size: 126 */
+real_t QDy[ 126 ];
 
-/** Matrix of size: 50 x 6 (row major format) */
-real_t H10[ 300 ];
+/** Matrix of size: 20 x 6 (row major format) */
+real_t H10[ 120 ];
 
-/** Matrix of size: 56 x 56 (row major format) */
-real_t H[ 3136 ];
+/** Matrix of size: 26 x 26 (row major format) */
+real_t H[ 676 ];
 
-/** Matrix of size: 50 x 56 (row major format) */
-real_t A[ 2800 ];
+/** Matrix of size: 20 x 26 (row major format) */
+real_t A[ 520 ];
 
-/** Column vector of size: 56 */
-real_t g[ 56 ];
+/** Column vector of size: 26 */
+real_t g[ 26 ];
 
-/** Column vector of size: 56 */
-real_t lb[ 56 ];
+/** Column vector of size: 26 */
+real_t lb[ 26 ];
 
-/** Column vector of size: 56 */
-real_t ub[ 56 ];
+/** Column vector of size: 26 */
+real_t ub[ 26 ];
 
-/** Column vector of size: 50 */
-real_t lbA[ 50 ];
+/** Column vector of size: 20 */
+real_t lbA[ 20 ];
 
-/** Column vector of size: 50 */
-real_t ubA[ 50 ];
+/** Column vector of size: 20 */
+real_t ubA[ 20 ];
 
-/** Column vector of size: 56 */
-real_t x[ 56 ];
+/** Column vector of size: 26 */
+real_t x[ 26 ];
 
-/** Column vector of size: 106 */
-real_t y[ 106 ];
+/** Column vector of size: 46 */
+real_t y[ 46 ];
 
 
 } ACADOworkspace;
@@ -270,10 +274,11 @@ real_t y[ 106 ];
  *
  *  \param rk_eta Working array to pass the input values and return the results.
  *  \param resetIntegrator The internal memory of the integrator can be reset.
+ *  \param rk_index Number of the shooting interval.
  *
  *  \return Status code of the integrator.
  */
-int acado_integrate( real_t* const rk_eta, int resetIntegrator );
+int acado_integrate( real_t* const rk_eta, int resetIntegrator, int rk_index );
 
 /** Export of an ACADO symbolic function.
  *
@@ -306,7 +311,7 @@ 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 strategy Shifting strategy: 1. Initialize node 21 with xEnd. 2. Initialize node 21 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.
  */

selfdrive/controls/lib/longitudinal_mpc/mpc_export/acado_integrator.c

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

@@ -40,7 +40,7 @@ int acado_solve( void )
 {
 	acado_nWSR = QPOASES_NWSRMAX;
 
-	QProblem qp(56, 50);
+	QProblem qp(26, 20);
 	
 	returnValue retVal = qp.init(acadoWorkspace.H, acadoWorkspace.g, acadoWorkspace.A, acadoWorkspace.lb, acadoWorkspace.ub, acadoWorkspace.lbA, acadoWorkspace.ubA, acado_nWSR, acadoWorkspace.y);
 

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

@@ -37,9 +37,9 @@
  */
 
 /** Maximum number of optimization variables. */
-#define QPOASES_NVMAX      56
+#define QPOASES_NVMAX      26
 /** Maximum number of constraints. */
-#define QPOASES_NCMAX      50
+#define QPOASES_NCMAX      20
 /** Maximum number of working set recalculations. */
 #define QPOASES_NWSRMAX    500
 /** Print level for qpOASES. */

selfdrive/controls/lib/planner.py

@@ -16,7 +16,7 @@ from selfdrive.controls.lib.drive_helpers import create_event, EventTypes as ET
 from selfdrive.controls.lib.pathplanner import PathPlanner
 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
+from selfdrive.controls.lib.longcontrol import LongCtrlState, MIN_CAN_SPEED
 
 _DT = 0.01    # 100Hz
 _DT_MPC = 0.2  # 5Hz
@@ -255,8 +255,9 @@ class Planner(object):
   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.poller = zmq.Poller()
+    self.live20 = messaging.sub_sock(context, service_list['live20'].port, conflate=True, poller=self.poller)
+    self.model = messaging.sub_sock(context, service_list['model'].port, conflate=True, poller=self.poller)
     self.plan = messaging.pub_sock(context, service_list['plan'].port)
     self.live_longitudinal_mpc = messaging.pub_sock(context, service_list['liveLongitudinalMpc'].port)
 
@@ -326,7 +327,15 @@ class Planner(object):
     cur_time = sec_since_boot()
     v_cruise_setpoint = v_cruise_kph * CV.KPH_TO_MS
 
-    md = messaging.recv_sock(self.model)
+    md = None
+    l20 = None
+
+    for socket, event in self.poller.poll(0):
+      if socket is self.model:
+        md = messaging.recv_one(socket)
+      elif socket is self.live20:
+        l20 = messaging.recv_one(socket)
+
     if md is not None:
       self.last_md_ts = md.logMonoTime
       self.last_model = cur_time
@@ -334,7 +343,6 @@ class Planner(object):
 
       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
@@ -368,17 +376,21 @@ class Planner(object):
                                                       accel_limits[1], accel_limits[0],
                                                       jerk_limits[1], jerk_limits[0],
                                                       _DT_MPC)
+        # cruise speed can't be negative even is user is distracted
+        self.v_cruise = max(self.v_cruise, 0.)
       else:
         starting = LoC.long_control_state == LongCtrlState.starting
         a_ego = min(CS.aEgo, 0.0)
-        self.v_cruise = CS.vEgo
-        self.a_cruise = self.CP.startAccel if starting else a_ego
-        self.v_acc_start = CS.vEgo
-        self.a_acc_start = self.CP.startAccel if starting else a_ego
-        self.v_acc = CS.vEgo
-        self.a_acc = self.CP.startAccel if starting else a_ego
-        self.v_acc_sol = CS.vEgo
-        self.a_acc_sol = self.CP.startAccel if starting else a_ego
+        reset_speed = MIN_CAN_SPEED if starting else CS.vEgo
+        reset_accel = self.CP.startAccel if starting else a_ego
+        self.v_acc = reset_speed
+        self.a_acc = reset_accel
+        self.v_acc_start = reset_speed
+        self.a_acc_start = reset_accel
+        self.v_cruise = reset_speed
+        self.a_cruise = reset_accel
+        self.v_acc_sol = reset_speed
+        self.a_acc_sol = reset_accel
 
       self.mpc1.set_cur_state(self.v_acc_start, self.a_acc_start)
       self.mpc2.set_cur_state(self.v_acc_start, self.a_acc_start)
@@ -399,7 +411,7 @@ class Planner(object):
                                          self.lead_1.fcw, blinkers) \
                  and not CS.brakePressed
       if self.fcw:
-        cloudlog.info("FCW triggered")
+        cloudlog.info("FCW triggered %s", self.fcw_checker.counters)
 
     if cur_time - self.last_model > 0.5:
       self.model_dead = True

selfdrive/controls/lib/speed_smoother.py

@@ -67,15 +67,20 @@ def speed_smoother(vEgo, aEgo, vT, aMax, aMin, jMax, jMin, ts):
   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
+    if aPeak <= 0: # there is no solution, so stop after t1
+      t2 = t1 + ts + 1e-9
+      t3 = t2
     else:
-      t2 = t1 + ts
+      vChange = dV - 0.5 * (aPeak**2 - aEgo**2) / jMax + 0.5 * aPeak**2 / jMin
+      if vChange < aPeak * ts:
+        t2 = t1 + vChange / aPeak
+      else:
+        t2 = t1 + ts
+      t3 = t2 - aPeak / jMin
   else:
     t1 = (aPeak - aEgo) / jMax
     t2 = t1
-  t3 = t2 - aPeak / jMin
+    t3 = t2 - aPeak / jMin
 
   dt1 = min(ts, t1)
   dt2 = max(min(ts, t2) - t1, 0.)

selfdrive/controls/lib/vehicle_model.py

@@ -89,6 +89,8 @@ class VehicleModel(object):
     A, B = create_dyn_state_matrices(u, self)
     return np.matmul((A * self.dt + np.identity(2)), self.state) + B * sa * self.dt
 
+  def yaw_rate(self, sa, u):
+    return self.calc_curvature(sa, u) * u
 
 if __name__ == '__main__':
   from selfdrive.car.honda.interface import CarInterface

selfdrive/controls/radard.py

@@ -44,11 +44,12 @@ class EKFV1D(EKF):
 
 # fuses camera and radar data for best lead detection
 def radard_thread(gctx=None):
-  set_realtime_priority(1)
+  set_realtime_priority(2)
 
   # wait for stats about the car to come in from controls
   cloudlog.info("radard is waiting for CarParams")
   CP = car.CarParams.from_bytes(Params().get("CarParams", block=True))
+  mocked= CP.radarName == "mock"
   VM = VehicleModel(CP)
   cloudlog.info("radard got CarParams")
 
@@ -59,8 +60,9 @@ def radard_thread(gctx=None):
   context = zmq.Context()
 
   # *** subscribe to features and model from visiond
-  model = messaging.sub_sock(context, service_list['model'].port)
-  live100 = messaging.sub_sock(context, service_list['live100'].port)
+  poller = zmq.Poller()
+  model = messaging.sub_sock(context, service_list['model'].port, conflate=True, poller=poller)
+  live100 = messaging.sub_sock(context, service_list['live100'].port, conflate=True, poller=poller)
 
   PP = PathPlanner()
   RI = RadarInterface()
@@ -96,7 +98,6 @@ def radard_thread(gctx=None):
 
   rk = Ratekeeper(rate, print_delay_threshold=np.inf)
   while 1:
-
     rr = RI.update()
 
     ar_pts = {}
@@ -104,7 +105,15 @@ def radard_thread(gctx=None):
       ar_pts[pt.trackId] = [pt.dRel + RDR_TO_LDR, pt.yRel, pt.vRel, pt.measured]
 
     # receive the live100s
-    l100 = messaging.recv_sock(live100)
+    l100 = None
+    md = None
+
+    for socket, event in poller.poll(0):
+      if socket is live100:
+        l100 = messaging.recv_one(socket)
+      elif socket is model:
+        md = messaging.recv_one(socket)
+
     if l100 is not None:
       active = l100.live100.active
       v_ego = l100.live100.vEgo
@@ -119,7 +128,6 @@ def radard_thread(gctx=None):
     if v_ego is None:
       continue
 
-    md = messaging.recv_sock(model)
     if md is not None:
       last_md_ts = md.logMonoTime
 
@@ -140,9 +148,11 @@ def radard_thread(gctx=None):
         del ar_pts[VISION_POINT]
 
     # *** compute the likely path_y ***
-    if active and not steer_override:    # use path from model path_poly
+    if (active and not steer_override) or mocked:
+      # use path from model (always when mocking as steering is too noisy)
       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
+    else:
+      # use path from steer, set angle_offset to 0 it does not only report the physical offset
       path_y = calc_lookahead_offset(v_ego, steer_angle, path_x, VM, angle_offset=0)[0]
 
     # *** remove missing points from meta data ***
@@ -152,8 +162,8 @@ def radard_thread(gctx=None):
 
     # *** compute the tracks ***
     for ids in ar_pts:
-      # ignore the vision point for now
-      if ids == VISION_POINT:
+      # ignore standalone vision point, unless we are mocking the radar
+      if ids == VISION_POINT and not mocked:
         continue
       rpt = ar_pts[ids]
 
@@ -185,33 +195,11 @@ def radard_thread(gctx=None):
         tracks[fused_id].vision_cnt += 1
         tracks[fused_id].update_vision_fusion()
 
-
-    # publish tracks (debugging)
-    dat = messaging.new_message()
-    dat.init('liveTracks', len(tracks))
-
     if DEBUG:
       print "NEW CYCLE"
       if VISION_POINT in ar_pts:
         print "vision", ar_pts[VISION_POINT]
 
-    for cnt, ids in enumerate(tracks.keys()):
-      if DEBUG:
-        print "id: %4.0f x:  %4.1f  y: %4.1f  vr: %4.1f d: %4.1f  va: %4.1f  vl: %4.1f  vlk: %4.1f alk: %4.1f  s: %1.0f" % \
-          (ids, tracks[ids].dRel, tracks[ids].yRel, tracks[ids].vRel,
-           tracks[ids].dPath, tracks[ids].vLat, 
-           tracks[ids].vLead, tracks[ids].vLeadK, 
-           tracks[ids].aLeadK, 
-           tracks[ids].stationary)
-      dat.liveTracks[cnt].trackId = ids
-      dat.liveTracks[cnt].dRel = float(tracks[ids].dRel)
-      dat.liveTracks[cnt].yRel = float(tracks[ids].yRel)
-      dat.liveTracks[cnt].vRel = float(tracks[ids].vRel)
-      dat.liveTracks[cnt].aRel = float(tracks[ids].aRel)
-      dat.liveTracks[cnt].stationary = tracks[ids].stationary
-      dat.liveTracks[cnt].oncoming = tracks[ids].oncoming
-    liveTracks.send(dat.to_bytes())
-
     idens = tracks.keys()
     track_pts = np.array([tracks[iden].get_key_for_cluster() for iden in idens])
 
@@ -268,6 +256,27 @@ def radard_thread(gctx=None):
     dat.live20.cumLagMs = -rk.remaining*1000.
     live20.send(dat.to_bytes())
 
+    # *** publish tracks for UI debugging (keep last) ***
+    dat = messaging.new_message()
+    dat.init('liveTracks', len(tracks))
+
+    for cnt, ids in enumerate(tracks.keys()):
+      if DEBUG:
+        print "id: %4.0f x:  %4.1f  y: %4.1f  vr: %4.1f d: %4.1f  va: %4.1f  vl: %4.1f  vlk: %4.1f alk: %4.1f  s: %1.0f" % \
+          (ids, tracks[ids].dRel, tracks[ids].yRel, tracks[ids].vRel,
+           tracks[ids].dPath, tracks[ids].vLat,
+           tracks[ids].vLead, tracks[ids].vLeadK,
+           tracks[ids].aLeadK,
+           tracks[ids].stationary)
+      dat.liveTracks[cnt].trackId = ids
+      dat.liveTracks[cnt].dRel = float(tracks[ids].dRel)
+      dat.liveTracks[cnt].yRel = float(tracks[ids].yRel)
+      dat.liveTracks[cnt].vRel = float(tracks[ids].vRel)
+      dat.liveTracks[cnt].aRel = float(tracks[ids].aRel)
+      dat.liveTracks[cnt].stationary = tracks[ids].stationary
+      dat.liveTracks[cnt].oncoming = tracks[ids].oncoming
+    liveTracks.send(dat.to_bytes())
+
     rk.monitor_time()
 
 def main(gctx=None):

selfdrive/loggerd/uploader.py

@@ -9,6 +9,7 @@ import inspect
 import requests
 import traceback
 import threading
+import subprocess
 
 from collections import Counter
 from selfdrive.swaglog import cloudlog
@@ -66,6 +67,13 @@ def clear_locks(root):
     except OSError:
       cloudlog.exception("clear_locks failed")
 
+def is_on_wifi():
+  # ConnectivityManager.getActiveNetworkInfo()
+  result = subprocess.check_output(["service", "call", "connectivity", "2"]).strip().split("\n")
+  data = ''.join(''.join(w.decode("hex")[::-1] for w in l[14:49].split()) for l in result[1:]) 
+
+  return "\x00".join("WIFI") in data
+
 
 class Uploader(object):
   def __init__(self, dongle_id, access_token, root):
@@ -111,21 +119,22 @@ class Uploader(object):
       total_size += os.stat(fn).st_size
     return dict(name_counts), total_size
 
-  def next_file_to_upload(self):
+  def next_file_to_upload(self, with_video):
     # try to upload log files first
     for name, key, fn in self.gen_upload_files():
       if name in ["rlog", "rlog.bz2"]:
         return (key, fn, 0)
 
-    # then upload compressed camera file
-    for name, key, fn in self.gen_upload_files():
-      if name in ["fcamera.hevc"]:
-        return (key, fn, 1)
+    if with_video:
+      # then upload compressed camera file
+      for name, key, fn in self.gen_upload_files():
+        if name in ["fcamera.hevc"]:
+          return (key, fn, 1)
 
-    # then upload other files
-    for name, key, fn in self.gen_upload_files():
-      if not name.endswith('.lock') and not name.endswith(".tmp"):
-        return (key, fn, 1)
+      # then upload other files
+      for name, key, fn in self.gen_upload_files():
+        if not name.endswith('.lock') and not name.endswith(".tmp"):
+          return (key, fn, 1)
 
     return None
 
@@ -240,13 +249,16 @@ def uploader_fn(exit_event):
   uploader = Uploader(dongle_id, access_token, ROOT)
 
   while True:
+
+    upload_video = (params.get("IsUploadVideoOverCellularEnabled") != "0") or is_on_wifi()
+
     backoff = 0.1
     while True:
 
       if exit_event.is_set():
         return
 
-      d = uploader.next_file_to_upload()
+      d = uploader.next_file_to_upload(upload_video)
       if d is None:
         break
 

selfdrive/manager.py

@@ -7,13 +7,9 @@ import errno
 import signal
 
 if __name__ == "__main__":
-  if os.path.isfile("/init.qcom.rc"):
-    # check if NEOS update is required
-    while ((not os.path.isfile("/VERSION")
-            or int(open("/VERSION").read()) < 3)
-            and not os.path.isfile("/data/media/0/noupdate")):
-      os.system("curl -o /tmp/updater https://neos.comma.ai/updater && chmod +x /tmp/updater && /tmp/updater")
-      time.sleep(10)
+  if os.path.isfile("/init.qcom.rc") \
+      and (not os.path.isfile("/VERSION") or int(open("/VERSION").read()) < 4):
+    raise Exception("NEOS outdated")
 
   # get a non-blocking stdout
   child_pid, child_pty = os.forkpty()
@@ -44,6 +40,7 @@ if __name__ == "__main__":
 
     os._exit(os.wait()[1])
 
+import shutil
 import hashlib
 import importlib
 import subprocess
@@ -219,22 +216,13 @@ def kill_managed_process(name):
 
 def cleanup_all_processes(signal, frame):
   cloudlog.info("caught ctrl-c %s %s" % (signal, frame))
-  manage_baseui(False)
+
+  for p in ("com.waze", "com.spotify.music", "ai.comma.plus.offroad", "ai.comma.plus.frame"):
+    system("am force-stop %s" % p)
+
   for name in running.keys():
     kill_managed_process(name)
-  sys.exit(0)
-
-baseui_running = False
-def manage_baseui(start):
-  global baseui_running
-  if start and not baseui_running:
-    cloudlog.info("starting baseui")
-    os.system("am start -n com.baseui/.MainActivity")
-    baseui_running = True
-  elif not start and baseui_running:
-    cloudlog.info("stopping baseui")
-    os.system("am force-stop com.baseui")
-    baseui_running = False
+  cloudlog.info("everything is dead")
 
 
 # ****************** run loop ******************
@@ -252,7 +240,11 @@ def manager_init():
   cloudlog.info("dongle id is " + dongle_id)
   os.environ['DONGLE_ID'] = dongle_id
 
-  dirty = subprocess.call(["git", "diff-index", "--quiet", "origin/release", "--"]) != 0
+  if "-private" in subprocess.check_output(["git", "config", "--get", "remote.origin.url"]):
+    upstream = "origin/master"
+  else:
+    upstream = "origin/release"
+  dirty = subprocess.call(["git", "diff-index", "--quiet", upstream, "--"]) != 0
   cloudlog.info("dirty is %d" % dirty)
   if not dirty:
     os.environ['CLEAN'] = '1'
@@ -280,12 +272,9 @@ def system(cmd):
       output=e.output[-1024:],
       returncode=e.returncode)
 
-# TODO: this is not proper gating for EON
-try:
+EON = os.path.exists("/EON")
+if EON:
   from smbus2 import SMBus
-  EON = True
-except ImportError:
-  EON = False
 
 def setup_eon_fan():
   if not EON:
@@ -322,8 +311,10 @@ _TEMP_THRS_H = [50., 65., 80., 10000]
 _TEMP_THRS_L = [42.5, 57.5, 72.5, 10000]
 # fan speed options
 _FAN_SPEEDS = [0, 16384, 32768, 65535]
+# max fan speed only allowed if battery if hot
+_BAT_TEMP_THERSHOLD = 45.
 
-def handle_fan(max_temp, fan_speed):
+def handle_fan(max_temp, bat_temp, fan_speed):
   new_speed_h = next(speed for speed, temp_h in zip(_FAN_SPEEDS, _TEMP_THRS_H) if temp_h > max_temp)
   new_speed_l = next(speed for speed, temp_l in zip(_FAN_SPEEDS, _TEMP_THRS_L) if temp_l > max_temp)
 
@@ -334,6 +325,10 @@ def handle_fan(max_temp, fan_speed):
     # update speed if using the low thresholds results in fan speed decrement
     fan_speed = new_speed_l
 
+  if bat_temp < _BAT_TEMP_THERSHOLD:
+    # no max fan speed unless battery is hot
+    fan_speed = min(fan_speed, _FAN_SPEEDS[-2])
+
   set_eon_fan(fan_speed/16384)
 
   return fan_speed
@@ -369,8 +364,6 @@ class LocationStarter(object):
       return location.speed*3.6 > 10
 
 def manager_thread():
-  global baseui_running
-
   # now loop
   context = zmq.Context()
   thermal_sock = messaging.pub_sock(context, service_list['thermal'].port)
@@ -383,7 +376,8 @@ def manager_thread():
   for p in persistent_processes:
     start_managed_process(p)
 
-  manage_baseui(True)
+  # start frame
+  system("am start -n ai.comma.plus.frame/.MainActivity")
 
   # do this before panda flashing
   setup_eon_fan()
@@ -391,7 +385,9 @@ def manager_thread():
   if os.getenv("NOBOARD") is None:
     start_managed_process("pandad")
 
-  passive = bool(os.getenv("PASSIVE"))
+  params = Params()
+
+  passive = params.get("Passive") == "1"
   passive_starter = LocationStarter()
 
   started_ts = None
@@ -399,6 +395,7 @@ def manager_thread():
   count = 0
   fan_speed = 0
   ignition_seen = False
+  battery_was_high = False
 
   health_sock.RCVTIMEO = 1500
 
@@ -424,13 +421,19 @@ def manager_thread():
       msg.thermal.batteryPercent = int(f.read())
     with open("/sys/class/power_supply/battery/status") as f:
       msg.thermal.batteryStatus = f.read().strip()
+    with open("/sys/class/power_supply/usb/online") as f:
+      msg.thermal.usbOnline = bool(int(f.read()))
 
     # TODO: add car battery voltage check
     max_temp = max(msg.thermal.cpu0, msg.thermal.cpu1,
                    msg.thermal.cpu2, msg.thermal.cpu3) / 10.0
-    fan_speed = handle_fan(max_temp, fan_speed)
+    bat_temp = msg.thermal.bat/1000.
+    fan_speed = handle_fan(max_temp, bat_temp, fan_speed)
     msg.thermal.fanSpeed = fan_speed
 
+    msg.thermal.started = started_ts is not None
+    msg.thermal.startedTs = int(1e9*(started_ts or 0))
+
     thermal_sock.send(msg.to_bytes())
     print msg
 
@@ -448,8 +451,10 @@ def manager_thread():
     ignition = td is not None and td.health.started
     ignition_seen = ignition_seen or ignition
 
-    params = Params()
-    should_start = ignition and (params.get("HasAcceptedTerms") == "1")
+    do_uninstall = params.get("DoUninstall") == "1"
+    accepted_terms = params.get("HasAcceptedTerms") == "1"
+
+    should_start = ignition
 
     # start on gps in passive mode
     if passive and not ignition_seen:
@@ -458,6 +463,15 @@ def manager_thread():
     # with 2% left, we killall, otherwise the phone is bricked
     should_start = should_start and avail > 0.02
 
+    # require usb power
+    should_start = should_start and msg.thermal.usbOnline
+
+    should_start = should_start and accepted_terms and (not do_uninstall)
+
+    # if any CPU gets above 107 or the battery gets above 53, kill all processes
+    # controls will warn with CPU above 95 or battery above 50
+    if max_temp > 107.0 or msg.thermal.bat >= 53000:
+      should_start = False
 
     if should_start:
       if not started_ts:
@@ -468,20 +482,17 @@ def manager_thread():
           kill_managed_process(p)
         else:
           start_managed_process(p)
-      manage_baseui(False)
     else:
-      manage_baseui(True)
       started_ts = None
       logger_dead = False
       for p in car_started_processes:
         kill_managed_process(p)
 
-      # shutdown if the battery gets lower than 10%, we aren't running, and we are discharging
-      if msg.thermal.batteryPercent < 5 and msg.thermal.batteryStatus == "Discharging":
+      # shutdown if the battery gets lower than 5%, we aren't running, and we are discharging
+      if msg.thermal.batteryPercent < 5 and msg.thermal.batteryStatus == "Discharging" and battery_was_high:
         os.system('LD_LIBRARY_PATH="" svc power shutdown')
-
-    # check the status of baseui
-    baseui_running = 'com.baseui' in subprocess.check_output(["ps"])
+      if msg.thermal.batteryPercent > 10:
+        battery_was_high = True
 
     # check the status of all processes, did any of them die?
     for p in running:
@@ -495,10 +506,13 @@ def manager_thread():
         health=(td.to_dict() if td else None),
         thermal=msg.to_dict())
 
+    if do_uninstall:
+      break
+
     count += 1
 
 def get_installed_apks():
-  dat = subprocess.check_output(["pm", "list", "packages", "-3", "-f"]).strip().split("\n")
+  dat = subprocess.check_output(["pm", "list", "packages", "-f"]).strip().split("\n")
   ret = {}
   for x in dat:
     if x.startswith("package:"):
@@ -506,21 +520,16 @@ def get_installed_apks():
       ret[k] = v
   return ret
 
-# optional, build the c++ binaries and preimport the python for speed
-def manager_prepare():
-  if os.path.exists(os.path.join(BASEDIR, ".gitmodules")):
-    # update submodules
-    system("cd %s && git submodule init panda opendbc pyextra" % BASEDIR)
-    system("cd %s && git submodule update panda opendbc pyextra" % BASEDIR)
+def install_apk(path):
+  # can only install from world readable path
+  install_path = "/sdcard/%s" % os.path.basename(path)
+  shutil.copyfile(path, install_path)
 
-  # build cereal first
-  subprocess.check_call(["make", "-j4"], cwd=os.path.join(BASEDIR, "cereal"))
-
-  # build all processes
-  os.chdir(os.path.dirname(os.path.abspath(__file__)))
-  for p in managed_processes:
-    prepare_managed_process(p)
+  ret = subprocess.call(["pm", "install", "-r", install_path])
+  os.remove(install_path)
+  return ret == 0
 
+def update_apks():
   # install apks
   installed = get_installed_apks()
   for app in os.listdir(os.path.join(BASEDIR, "apk/")):
@@ -529,7 +538,8 @@ def manager_prepare():
       if app not in installed:
         installed[app] = None
   cloudlog.info("installed apks %s" % (str(installed), ))
-  for app in installed:
+
+  for app in installed.iterkeys():
     apk_path = os.path.join(BASEDIR, "apk/"+app+".apk")
     if os.path.isfile(apk_path):
       h1 = hashlib.sha1(open(apk_path).read()).hexdigest()
@@ -537,16 +547,37 @@ def manager_prepare():
       if installed[app] is not None:
         h2 = hashlib.sha1(open(installed[app]).read()).hexdigest()
         cloudlog.info("comparing version of %s  %s vs %s" % (app, h1, h2))
+
       if h2 is None or h1 != h2:
         cloudlog.info("installing %s" % app)
-        for do_uninstall in [False, True]:
-          if do_uninstall:
-            cloudlog.info("needing to uninstall %s" % app)
-            os.system("pm uninstall %s" % app)
-          ret = os.system("cp %s /sdcard/%s.apk && pm install -r /sdcard/%s.apk && rm /sdcard/%s.apk" % (apk_path, app, app, app))
-          if ret == 0:
-            break
-        assert ret == 0
+
+        success = install_apk(apk_path)
+        if not success:
+          cloudlog.info("needing to uninstall %s" % app)
+          system("pm uninstall %s" % app)
+          success = install_apk(apk_path)
+
+        assert success
+
+def manager_update():
+  update_apks()
+
+def manager_prepare():
+
+  # build cereal first
+  subprocess.check_call(["make", "-j4"], cwd=os.path.join(BASEDIR, "cereal"))
+
+  # build all processes
+  os.chdir(os.path.dirname(os.path.abspath(__file__)))
+  for p in managed_processes:
+    prepare_managed_process(p)
+
+def uninstall():
+  cloudlog.warning("uninstalling")
+  with open('/cache/recovery/command', 'w') as f:
+    f.write('--wipe_data\n')
+  # IPowerManager.reboot(confirm=false, reason="recovery", wait=true)
+  os.system("service call power 16 i32 0 s16 recovery i32 1")  
 
 def main():
   if os.getenv("NOLOG") is not None:
@@ -581,11 +612,13 @@ def main():
   if params.get("IsMetric") is None:
     params.put("IsMetric", "0")
   if params.get("IsRearViewMirror") is None:
-    params.put("IsRearViewMirror", "1")
+    params.put("IsRearViewMirror", "0")
   if params.get("IsFcwEnabled") is None:
     params.put("IsFcwEnabled", "1")
   if params.get("HasAcceptedTerms") is None:
     params.put("HasAcceptedTerms", "0")
+  if params.get("IsUploadVideoOverCellularEnabled") is None:
+    params.put("IsUploadVideoOverCellularEnabled", "1")
 
   params.put("Passive", "1" if os.getenv("PASSIVE") else "0")
 
@@ -597,6 +630,7 @@ def main():
       cwd=os.path.join(BASEDIR, "selfdrive", "ui", "spinner"),
       close_fds=True)
   try:
+    manager_update()
     manager_init()
     manager_prepare()
   finally:
@@ -606,6 +640,9 @@ def main():
   if os.getenv("PREPAREONLY") is not None:
     return
 
+  # SystemExit on sigterm
+  signal.signal(signal.SIGTERM, lambda signum, frame: sys.exit(1))
+
   try:
     manager_thread()
   except Exception:
@@ -614,6 +651,9 @@ def main():
   finally:
     cleanup_all_processes(None, None)
 
+  if params.get("DoUninstall") == "1":
+    uninstall()
+
 if __name__ == "__main__":
   main()
   # manual exit because we are forked

selfdrive/radar/mock/interface.py

@@ -0,0 +1,24 @@
+#!/usr/bin/env python
+from cereal import car
+import time
+
+
+class RadarInterface(object):
+  def __init__(self):
+    # radar
+    self.pts = {}
+    self.delay = 0.1
+
+  def update(self):
+
+    ret = car.RadarState.new_message()
+    time.sleep(0.05)  # radard runs on RI updates
+    
+    return ret
+
+if __name__ == "__main__":
+  RI = RadarInterface()
+  while 1:
+    ret = RI.update()
+    print(chr(27) + "[2J")
+    print ret

selfdrive/radar/nidec/interface.py

@@ -33,7 +33,6 @@ class RadarInterface(object):
     self.radar_fault = False
 
     self.delay = 0.1  # Delay of radar
-    self.cutin_prediction = True
 
     # Nidec
     self.rcp = _create_nidec_can_parser()

selfdrive/radar/toyota/interface.py

@@ -34,7 +34,6 @@ class RadarInterface(object):
 
     # Nidec
     self.rcp = _create_radard_can_parser()
-    self.cutin_prediction = False
 
     context = zmq.Context()
     self.logcan = messaging.sub_sock(context, service_list['can'].port)

selfdrive/registration.py

@@ -19,16 +19,23 @@ def get_git_commit():
 def get_git_branch():
   return subprocess.check_output(["git", "rev-parse", "--abbrev-ref", "HEAD"]).strip()
 
+def get_git_remote():
+  return subprocess.check_output(["git", "config", "--get", "remote.origin.url"]).strip()
+
 def register():
   params = Params()
-  try:
-    params.put("Version", version)
-    params.put("GitCommit", get_git_commit())
-    params.put("GitBranch", get_git_branch())
+  params.put("Version", version)
+  params.put("GitCommit", get_git_commit())
+  params.put("GitBranch", get_git_branch())
+  params.put("GitRemote", get_git_remote())
 
-    dongle_id, access_token = params.get("DongleId"), params.get("AccessToken")
+  dongle_id, access_token = params.get("DongleId"), params.get("AccessToken")
+
+  try:
     if dongle_id is None or access_token is None:
-      resp = api_get("v1/pilotauth/", method='POST', imei=get_imei(), serial=get_serial())
+      cloudlog.info("getting pilotauth")
+      resp = api_get("v1/pilotauth/", method='POST', timeout=15,
+                     imei=get_imei(), serial=get_serial())
       dongleauth = json.loads(resp.text)
       dongle_id, access_token = dongleauth["dongle_id"].encode('ascii'), dongleauth["access_token"].encode('ascii')
 

selfdrive/service_list.yaml

@@ -46,6 +46,8 @@ ubloxGnss: [8033, true]
 clocks: [8034, true]
 liveMpc: [8035, true]
 liveLongitudinalMpc: [8036, true]
+plusFrame: [8037, false]
+navStatus: [8038, true]
 
 testModel: [8040, false]
 

selfdrive/test/plant/maneuver.py

@@ -60,6 +60,7 @@ class Maneuver(object):
           gas=gas, brake=brake, steer_torque=steer_torque,
           distance=distance, speed=speed, acceleration=acceleration,
           up_accel_cmd=last_live100.upAccelCmd, ui_accel_cmd=last_live100.uiAccelCmd,
+          uf_accel_cmd=last_live100.ufAccelCmd,
           d_rel=d_rel, v_rel=v_rel, v_lead=speed_lead,
           v_target_lead=last_live100.vTargetLead, pid_speed=last_live100.vPid,
           cruise_speed=last_live100.vCruise,

selfdrive/test/plant/maneuverplots.py

@@ -20,6 +20,7 @@ class ManeuverPlot(object):
 
     self.up_accel_cmd_array = []
     self.ui_accel_cmd_array = []
+    self.uf_accel_cmd_array = []
 
     self.d_rel_array = []
     self.v_rel_array = []
@@ -38,8 +39,8 @@ class ManeuverPlot(object):
     self.title = title
     
   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, fcw):
+    acceleration, up_accel_cmd, ui_accel_cmd, uf_accel_cmd, d_rel, v_rel, 
+    v_lead, v_target_lead, pid_speed, cruise_speed, jerk_factor, a_target, fcw):
     self.time_array.append(time)
     self.gas_array.append(gas)
     self.brake_array.append(brake)
@@ -49,6 +50,7 @@ class ManeuverPlot(object):
     self.acceleration_array.append(acceleration)
     self.up_accel_cmd_array.append(up_accel_cmd)
     self.ui_accel_cmd_array.append(ui_accel_cmd)
+    self.uf_accel_cmd_array.append(uf_accel_cmd)
     self.d_rel_array.append(d_rel)
     self.v_rel_array.append(v_rel)
     self.v_lead_array.append(v_lead)
@@ -117,12 +119,13 @@ class ManeuverPlot(object):
     plt.figure(plt_num)
     plt.plot(
       np.array(self.time_array), np.array(self.up_accel_cmd_array), 'g',
-      np.array(self.time_array), np.array(self.ui_accel_cmd_array), 'b'
+      np.array(self.time_array), np.array(self.ui_accel_cmd_array), 'b',
+      np.array(self.time_array), np.array(self.uf_accel_cmd_array), 'r'
     )
     plt.xlabel("Time, [s]")
     plt.ylabel("Accel Cmd [m/s^2]")
     plt.grid()
-    plt.legend(["Proportional", "Integral"], loc=0)
+    plt.legend(["Proportional", "Integral", "feedforward"], loc=0)
     pylab.savefig("/".join([path, maneuver_name, "pid.svg"]), dpi=1000)
 
     # relative distances chart =======

selfdrive/test/test_openpilot.py

@@ -4,7 +4,6 @@ os.environ['FAKEUPLOAD'] = "1"
 from common.testing import phone_only
 from selfdrive.manager import manager_init, manager_prepare
 from selfdrive.manager import start_managed_process, kill_managed_process, get_running
-from selfdrive.manager import manage_baseui
 from selfdrive.config import CruiseButtons
 from functools import wraps
 import time
@@ -93,10 +92,3 @@ def test_ui():
 def test_uploader():
   print "UPLOADER"
   time.sleep(10.0)
-
-@phone_only
-def test_baseui():
-  manage_baseui(True)
-  time.sleep(10.0)
-  manage_baseui(False)
-

selfdrive/ui/Makefile

@@ -52,6 +52,7 @@ ui: $(OBJS)
 	      $(CEREAL_LIBS) \
 	      $(ZMQ_LIBS) \
         -L/system/vendor/lib64 \
+        -lhardware \
         $(OPENGL_LIBS) \
         -lcutils -lm -llog
 

selfdrive/updated.py

@@ -7,9 +7,6 @@ import time
 import subprocess
 
 def main(gctx=None):
-  if not os.getenv("CLEAN"):
-    return
-
   while True:
     # try network
     r = subprocess.call(["ping", "-W", "4", "-c", "1", "8.8.8.8"])