openpilot v0.5.4 release

old-commit-hash: a422246dc3

Dockerfile.openpilot

@@ -17,3 +17,5 @@ COPY ./phonelibs /tmp/openpilot/phonelibs
 COPY ./pyextra /tmp/openpilot/pyextra
 
 RUN mkdir -p /tmp/openpilot/selfdrive/test/out
+RUN make -C /tmp/openpilot/selfdrive/controls/lib/longitudinal_mpc clean
+RUN make -C /tmp/openpilot/selfdrive/controls/lib/lateral_mpc clean

RELEASES.md

@@ -1,3 +1,14 @@
+Version 0.5.4 (2018-09-25)
+========================
+ * New Driving Model
+ * New Driver Monitoring Model
+ * Improve longitudinal mpc in mid-low speed braking
+ * Honda Accord hybrid support thanks to energee!
+ * Ship mpc binaries and sensibly reduce build time
+ * Calibration more stable
+ * More Hyundai and Kia cars supported thanks to emmertex!
+ * Various GM Volt improvements thanks to vntarasov!
+
 Version 0.5.3 (2018-09-03)
 ========================
  * Hyundai Santa Fe support!

cereal/car.capnp

@@ -42,7 +42,7 @@ struct CarEvent @0x9b1657f34caf3ad3 {
     speedTooLow @17;
     outOfSpace @18;
     overheat @19;
-    calibrationInProgress @20;
+    calibrationIncomplete @20;
     calibrationInvalid @21;
     controlsMismatch @22;
     pcmEnable @23;
@@ -69,6 +69,8 @@ struct CarEvent @0x9b1657f34caf3ad3 {
     promptDriverUnresponsive @44;
     driverUnresponsive @45;
     belowSteerSpeed @46;
+    calibrationProgress @47;
+    lowBattery @48;
   }
 }
 

cereal/log.capnp

@@ -276,6 +276,7 @@ struct ThermalData {
   startedTs @13 :UInt64;
 
   thermalStatus @14 :ThermalStatus;
+  chargerDisabled @17 :Bool;
 
   enum ThermalStatus {
     green @0;   # all processes run

common/dbc.py

@@ -1,7 +1,6 @@
 import re
 import os
 import struct
-import bitstring
 import sys
 import numbers
 from collections import namedtuple, defaultdict
@@ -17,6 +16,7 @@ DBCSignal = namedtuple(
   "DBCSignal", ["name", "start_bit", "size", "is_little_endian", "is_signed",
                 "factor", "offset", "tmin", "tmax", "units"])
 
+
 class dbc(object):
   def __init__(self, fn):
     self.name, _ = os.path.splitext(os.path.basename(fn))
@@ -122,6 +122,16 @@ class dbc(object):
       msg_id = self.msg_name_to_address[msg_id]
     return msg_id
 
+  def reverse_bytes(self, x):
+    return ((x & 0xff00000000000000) >> 56) | \
+           ((x & 0x00ff000000000000) >> 40) | \
+           ((x & 0x0000ff0000000000) >> 24) | \
+           ((x & 0x000000ff00000000) >> 8) | \
+           ((x & 0x00000000ff000000) << 8) | \
+           ((x & 0x0000000000ff0000) << 24) | \
+           ((x & 0x000000000000ff00) << 40) | \
+           ((x & 0x00000000000000ff) << 56)
+
   def encode(self, msg_id, dd):
     """Encode a CAN message using the dbc.
 
@@ -131,35 +141,40 @@ class dbc(object):
     """
     msg_id = self.lookup_msg_id(msg_id)
 
-    # TODO: Stop using bitstring, which is super slow.
     msg_def = self.msgs[msg_id]
     size = msg_def[0][1]
 
-    bsf = bitstring.Bits(hex="00"*size)
+    result = 0
     for s in msg_def[1]:
       ival = dd.get(s.name)
       if ival is not None:
-        ival = (ival / s.factor) - s.offset
-        ival = int(round(ival))
 
-        # should pack this
+        b2 = s.size
         if s.is_little_endian:
-          ss = s.start_bit
+          b1 = s.start_bit
         else:
-          ss = self.bits_index[s.start_bit]
+          b1 = (s.start_bit // 8) * 8 + (-s.start_bit - 1) % 8
+        bo = 64 - (b1 + s.size)
+
+        ival = (ival / s.factor) - s.offset
+        ival = int(round(ival))
 
+        if s.is_signed and ival < 0:
+          ival = (1 << b2) + ival
 
-        if s.is_signed:
+        shift = b1 if s.is_little_endian else bo
-          tbs = bitstring.Bits(int=ival, length=s.size)
+        mask = ((1 << b2) - 1) << shift
-        else:
+        dat = (ival & ((1 << b2) - 1)) << shift
-          tbs = bitstring.Bits(uint=ival, length=s.size)
+
+        if s.is_little_endian:
+          mask = self.reverse_bytes(mask)
+          dat = self.reverse_bytes(dat)
 
-        lpad = bitstring.Bits(bin="0b"+"0"*ss)
+        result &= ~mask
-        rpad = bitstring.Bits(bin="0b"+"0"*(8*size-(ss+s.size)))
+        result |= dat
-        tbs = lpad+tbs+rpad
 
-        bsf |= tbs
+    result = struct.pack('>Q', result)
-    return bsf.tobytes()
+    return result[:size]
 
   def decode(self, x, arr=None, debug=False):
     """Decode a CAN message using the dbc.
@@ -195,55 +210,77 @@ class dbc(object):
     if debug:
       print name
 
-    blen = 8*len(x[2])
+    st = x[2].ljust(8, '\x00')
-
-    st = x[2].rjust(8, '\x00')
     le, be = None, None
-    size = msg[0][1]
 
     for s in msg[1]:
       if arr is not None and s[0] not in arr:
         continue
 
-      # big or little endian?
+      start_bit = s[1]
-      #   see http://vi-firmware.openxcplatform.com/en/master/config/bit-numbering.html
+      signal_size = s[2]
-      if s[3] is False:
+      little_endian = s[3]
-        ss = self.bits_index[s[1]]
+      signed = s[4]
-        if be is None:
+      factor = s[5]
-          be = struct.unpack(">Q", st)[0]
+      offset = s[6]
-        x2_int = be
+
-        data_bit_pos = (blen - (ss + s[2]))
+      b2 = signal_size
+      if little_endian:
+        b1 = start_bit
       else:
+        b1 = (start_bit // 8) * 8 + (-start_bit - 1) % 8
+      bo = 64 - (b1 + signal_size)
+
+      if little_endian:
         if le is None:
           le = struct.unpack("<Q", st)[0]
-        x2_int = le >> (64 - 8 * size)
+        shift_amount = b1
-        ss = s[1]
+        tmp = le
-        data_bit_pos = ss
+      else:
+        if be is None:
+          be = struct.unpack(">Q", st)[0]
+        shift_amount = bo
+        tmp = be
 
-      if data_bit_pos < 0:
+      if shift_amount < 0:
         continue
-      ival = (x2_int >> data_bit_pos) & ((1 << (s[2])) - 1)
 
-      if s[4] and (ival & (1<<(s[2]-1))): # signed
+      tmp = (tmp >> shift_amount) & ((1 << b2) - 1)
-        ival -= (1<<s[2])
+      if signed and (tmp >> (b2 - 1)):
+        tmp -= (1 << b2)
+
+      tmp = tmp * factor + offset
 
-      # control the offset
-      ival = (ival * s[5]) + s[6]
       # if debug:
-      #  print "%40s  %2d %2d  %7.2f %s" % (s[0], s[1], s[2], ival, s[-1])
+      #   print "%40s  %2d %2d  %7.2f %s" % (s[0], s[1], s[2], tmp, s[-1])
 
       if arr is None:
-        out[s[0]] = ival
+        out[s[0]] = tmp
       else:
-        out[arr.index(s[0])] = ival
+        out[arr.index(s[0])] = tmp
     return name, out
 
   def get_signals(self, msg):
     msg = self.lookup_msg_id(msg)
     return [sgs.name for sgs in self.msgs[msg][1]]
 
+
 if __name__ == "__main__":
    from opendbc import DBC_PATH
+   import numpy as np
+
+   dbc_test = dbc(os.path.join(DBC_PATH, 'toyota_prius_2017_pt_generated.dbc'))
+   msg = ('STEER_ANGLE_SENSOR', {'STEER_ANGLE': -6.0, 'STEER_RATE': 4, 'STEER_FRACTION': -0.2})
+   encoded = dbc_test.encode(*msg)
+   decoded = dbc_test.decode((0x25, 0, encoded))
+   assert decoded == msg
+
+   dbc_test = dbc(os.path.join(DBC_PATH, 'hyundai_santa_fe_2019_ccan.dbc'))
+   decoded = dbc_test.decode((0x2b0, 0, "\xfa\xfe\x00\x07\x12"))
+   assert np.isclose(decoded[1]['SAS_Angle'], -26.2)
+
+   msg = ('SAS11', {'SAS_Stat': 7.0, 'MsgCount': 0.0, 'SAS_Angle': -26.200000000000003, 'SAS_Speed': 0.0, 'CheckSum': 0.0})
+   encoded = dbc_test.encode(*msg)
+   decoded = dbc_test.decode((0x2b0, 0, encoded))
 
-   dbc_test = dbc(os.path.join(DBC_PATH, sys.argv[1]))
+   assert decoded == msg
-   print dbc_test.get_signals(0xe4)

common/filter_simple.py

@@ -0,0 +1,10 @@
+class FirstOrderFilter():
+  # first order filter
+  def __init__(self, x0, ts, dt):
+    self.k = (dt / ts) / (1. + dt / ts)
+    self.x = x0
+
+  def update(self, x):
+    self.x = (1. - self.k) * self.x + self.k * x
+
+

common/transformations/model.py

@@ -57,6 +57,8 @@ model_frame_from_road_frame = np.dot(model_intrinsics,
 bigmodel_frame_from_road_frame = np.dot(bigmodel_intrinsics,
   get_view_frame_from_road_frame(0, 0, 0, model_height))
 
+model_frame_from_bigmodel_frame = np.dot(model_intrinsics, np.linalg.inv(bigmodel_intrinsics))
+
 # 'camera from model camera'
 def get_model_height_transform(camera_frame_from_road_frame, height):
   camera_frame_from_road_ground = np.dot(camera_frame_from_road_frame, np.array([

selfdrive/boardd/Makefile

@@ -13,8 +13,8 @@ WARN_FLAGS = -Werror=implicit-function-declaration \
              -Werror=return-type \
              -Werror=format-extra-args
 
-CFLAGS = -std=gnu11 -g -fPIC -I../../ -O2 $(WARN_FLAGS)
+CFLAGS = -std=gnu11 -g -fPIC -I../ -I../../ -O2 $(WARN_FLAGS)
-CXXFLAGS = -std=c++11 -g -fPIC -I../../ -O2 $(WARN_FLAGS)
+CXXFLAGS = -std=c++11 -g -fPIC -I../ -I../../ -O2 $(WARN_FLAGS)
 
 ZMQ_FLAGS = -I$(PHONELIBS)/zmq/aarch64/include
 ZMQ_LIBS = -L$(PHONELIBS)/zmq/aarch64/lib \

selfdrive/car/gm/carcontroller.py

@@ -4,7 +4,7 @@ from selfdrive.config import Conversions as CV
 from selfdrive.boardd.boardd import can_list_to_can_capnp
 from selfdrive.car import apply_std_steer_torque_limits
 from selfdrive.car.gm import gmcan
-from selfdrive.car.gm.values import CAR, DBC
+from selfdrive.car.gm.values import CAR, DBC, AccState
 from selfdrive.can.packer import CANPacker
 
 
@@ -29,11 +29,11 @@ class CarControllerParams():
     self.ADAS_KEEPALIVE_STEP = 10
     # pedal lookups, only for Volt
     MAX_GAS = 3072              # Only a safety limit
-    ZERO_GAS = 2048
+    self.ZERO_GAS = 2048
     MAX_BRAKE = 350             # Should be around 3.5m/s^2, including regen
     self.MAX_ACC_REGEN = 1404  # ACC Regen braking is slightly less powerful than max regen paddle
     self.GAS_LOOKUP_BP = [-0.25, 0., 0.5]
-    self.GAS_LOOKUP_V = [self.MAX_ACC_REGEN, ZERO_GAS, MAX_GAS]
+    self.GAS_LOOKUP_V = [self.MAX_ACC_REGEN, self.ZERO_GAS, MAX_GAS]
     self.BRAKE_LOOKUP_BP = [-1., -0.25]
     self.BRAKE_LOOKUP_V = [MAX_BRAKE, 0]
 
@@ -83,7 +83,6 @@ class CarController(object):
       return
 
     P = self.params
-
     # Send CAN commands.
     can_sends = []
     canbus = self.canbus
@@ -131,12 +130,18 @@ class CarController(object):
       if (frame % 4) == 0:
         idx = (frame / 4) % 4
 
-        at_full_stop = enabled and CS.standstill
+        car_stopping = apply_gas < P.ZERO_GAS
-        near_stop = enabled and (CS.v_ego < P.NEAR_STOP_BRAKE_PHASE)
+        standstill = CS.pcm_acc_status == AccState.STANDSTILL
+        at_full_stop = enabled and standstill and car_stopping
+        near_stop = enabled and (CS.v_ego < P.NEAR_STOP_BRAKE_PHASE) and car_stopping
         can_sends.append(gmcan.create_friction_brake_command(self.packer_ch, canbus.chassis, apply_brake, idx, near_stop, at_full_stop))
 
-        at_full_stop = enabled and CS.standstill
+        # Auto-resume from full stop by resetting ACC control
-        can_sends.append(gmcan.create_gas_regen_command(self.packer_pt, canbus.powertrain, apply_gas, idx, enabled, at_full_stop))
+        acc_enabled = enabled
+        if standstill and not car_stopping:
+          acc_enabled = False
+
+        can_sends.append(gmcan.create_gas_regen_command(self.packer_pt, canbus.powertrain, apply_gas, idx, acc_enabled, at_full_stop))
 
       # Send dashboard UI commands (ACC status), 25hz
       if (frame % 4) == 0:

selfdrive/car/gm/gmcan.py

@@ -60,17 +60,15 @@ def create_gas_regen_command(packer, bus, throttle, idx, acc_engaged, at_full_st
 
 def create_friction_brake_command(packer, bus, apply_brake, idx, near_stop, at_full_stop):
 
-  if apply_brake == 0:
   mode = 0x1
-  else:
+  if apply_brake > 0:
     mode = 0xa
 
+  if near_stop:
+    mode = 0xb
+
   if at_full_stop:
     mode = 0xd
-    # TODO: this is to have GM bringing the car to complete stop,
-    # but currently it conflicts with OP controls, so turned off.
-    #elif near_stop:
-    #  mode = 0xb
 
   brake = (0x1000 - apply_brake) & 0xfff
   checksum = (0x10000 - (mode << 12) - brake - idx) & 0xffff

selfdrive/car/gm/interface.py

@@ -4,7 +4,7 @@ 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
 from selfdrive.controls.lib.vehicle_model import VehicleModel
-from selfdrive.car.gm.values import DBC, CAR
+from selfdrive.car.gm.values import DBC, CAR, STOCK_CONTROL_MSGS
 from selfdrive.car.gm.carstate import CarState, CruiseButtons, get_powertrain_can_parser
 
 try:
@@ -28,10 +28,6 @@ class CanBus(object):
     self.chassis = 2
     self.sw_gmlan = 3
 
-# 384 = "ASCMLKASteeringCmd"
-# 715 = "ASCMGasRegenCmd"
-CONTROL_MSGS = [384, 715]
-
 class CarInterface(object):
   def __init__(self, CP, sendcan=None):
     self.CP = CP
@@ -74,7 +70,7 @@ class CarInterface(object):
     # Presence of a camera on the object bus is ok.
     # Have to go passive if ASCM is online (ACC-enabled cars),
     # or camera is on powertrain bus (LKA cars without ACC).
-    ret.enableCamera = not any(x for x in CONTROL_MSGS if x in fingerprint)
+    ret.enableCamera = not any(x for x in STOCK_CONTROL_MSGS[candidate] if x in fingerprint)
 
     std_cargo = 136
 
@@ -197,7 +193,7 @@ class CarInterface(object):
     ret.cruiseState.available = bool(self.CS.main_on)
     cruiseEnabled = self.CS.pcm_acc_status != 0
     ret.cruiseState.enabled = cruiseEnabled
-    ret.cruiseState.standstill = self.CS.pcm_acc_status == 4
+    ret.cruiseState.standstill = False
 
     ret.leftBlinker = self.CS.left_blinker_on
     ret.rightBlinker = self.CS.right_blinker_on
@@ -280,8 +276,6 @@ class CarInterface(object):
         events.append(create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE]))
       if ret.gasPressed:
         events.append(create_event('pedalPressed', [ET.PRE_ENABLE]))
-      if ret.cruiseState.standstill:
-        events.append(create_event('resumeRequired', [ET.WARNING]))
 
       # handle button presses
       for b in ret.buttonEvents:

selfdrive/car/gm/values.py

@@ -12,6 +12,12 @@ class CruiseButtons:
   MAIN        = 5
   CANCEL      = 6
 
+class AccState:
+  OFF        = 0
+  ACTIVE     = 1
+  FAULTED    = 3
+  STANDSTILL = 4
+
 def is_eps_status_ok(eps_status, car_fingerprint):
   valid_eps_status = []
   if car_fingerprint == CAR.VOLT:
@@ -49,6 +55,12 @@ FINGERPRINTS = {
 
 STEER_THRESHOLD = 1.0
 
+
+STOCK_CONTROL_MSGS = {
+  CAR.VOLT: [384, 715], # 384 = "ASCMLKASteeringCmd", 715 = "ASCMGasRegenCmd"
+  CAR.CADILLAC_CT6: [], # Cadillac does not require ASCMs to be disconnected
+}
+
 DBC = {
   CAR.VOLT: dbc_dict('gm_global_a_powertrain', 'gm_global_a_object', chassis_dbc='gm_global_a_chassis'),
   CAR.CADILLAC_CT6: dbc_dict('cadillac_ct6_powertrain', 'cadillac_ct6_object', chassis_dbc='cadillac_ct6_chassis'),

selfdrive/car/hyundai/interface.py

@@ -70,13 +70,18 @@ class CarInterface(object):
     tireStiffnessRear_civic = 202500
 
     ret.steerActuatorDelay = 0.1  # Default delay
+    tire_stiffness_factor = 1.
 
     if candidate == CAR.SANTA_FE:
       ret.steerKf = 0.00005
       ret.steerRateCost = 0.5
       ret.mass = 3982 * CV.LB_TO_KG + std_cargo
       ret.wheelbase = 2.766
-      ret.steerRatio = 13.8 * 1.15   # 15% higher at the center seems reasonable
+
+      # Values from optimizer
+      ret.steerRatio = 16.55  # 13.8 is spec end-to-end
+      tire_stiffness_factor = 0.82
+
       ret.steerKiBP, ret.steerKpBP = [[0.], [0.]]
       ret.steerKpV, ret.steerKiV = [[0.37], [0.1]]
       ret.minSteerSpeed = 0.
@@ -122,7 +127,6 @@ class CarInterface(object):
     ret.longitudinalKpV = [0.]
     ret.longitudinalKiBP = [0.]
     ret.longitudinalKiV = [0.]
-    tire_stiffness_factor = 1.
 
     ret.centerToFront = ret.wheelbase * 0.4
 

selfdrive/car/toyota/carcontroller.py

@@ -145,6 +145,9 @@ class CarController(object):
     # only cut torque when steer state is a known fault
     if not enabled or CS.steer_state in [9, 25]:
       apply_steer = 0
+      apply_steer_req = 0
+    else:
+      apply_steer_req = 1
 
     self.steer_angle_enabled, self.ipas_reset_counter = \
       ipas_state_transition(self.steer_angle_enabled, enabled, CS.ipas_active, self.ipas_reset_counter)
@@ -192,9 +195,9 @@ class CarController(object):
     # on consecutive messages
     if ECU.CAM in self.fake_ecus:
       if self.angle_control:
-        can_sends.append(create_steer_command(self.packer, 0., frame))
+        can_sends.append(create_steer_command(self.packer, 0., 0, frame))
       else:
-        can_sends.append(create_steer_command(self.packer, apply_steer, frame))
+        can_sends.append(create_steer_command(self.packer, apply_steer, apply_steer_req, frame))
 
     if self.angle_control:
       can_sends.append(create_ipas_steer_command(self.packer, apply_angle, self.steer_angle_enabled,

selfdrive/car/toyota/toyotacan.py

@@ -52,11 +52,11 @@ def create_ipas_steer_command(packer, steer, enabled, apgs_enabled):
     return packer.make_can_msg("STEERING_IPAS_COMMA", 0, values)
 
 
-def create_steer_command(packer, steer, raw_cnt):
+def create_steer_command(packer, steer, steer_req, raw_cnt):
   """Creates a CAN message for the Toyota Steer Command."""
 
   values = {
-    "STEER_REQUEST": abs(steer) > 0.001,
+    "STEER_REQUEST": steer_req,
     "STEER_TORQUE_CMD": steer,
     "COUNTER": raw_cnt,
     "SET_ME_1": 1,

selfdrive/common/ipc.c

@@ -0,0 +1,119 @@
+#include <stdio.h>
+#include <stdlib.h>
+#include <stdbool.h>
+#include <string.h>
+#include <unistd.h>
+#include <assert.h>
+#include <errno.h>
+
+#include <sys/mman.h>
+#include <sys/socket.h>
+#include <sys/un.h>
+
+#include "ipc.h"
+
+int ipc_connect(const char* socket_path) {
+  int err;
+
+  int sock = socket(AF_UNIX, SOCK_SEQPACKET, 0);
+  assert(sock >= 0);
+  struct sockaddr_un addr = {
+    .sun_family = AF_UNIX,
+  };
+  snprintf(addr.sun_path, sizeof(addr.sun_path), "%s", socket_path);
+  err = connect(sock, (struct sockaddr*)&addr, sizeof(addr));
+  if (err != 0) {
+    close(sock);
+    return -1;
+  }
+
+  return sock;
+}
+
+int ipc_bind(const char* socket_path) {
+  int err;
+
+  unlink(socket_path);
+
+  int sock = socket(AF_UNIX, SOCK_SEQPACKET, 0);
+  struct sockaddr_un addr = {
+    .sun_family = AF_UNIX,
+  };
+  snprintf(addr.sun_path, sizeof(addr.sun_path), "%s", socket_path);
+  err = bind(sock, (struct sockaddr *)&addr, sizeof(addr));
+  assert(err == 0);
+
+  err = listen(sock, 3);
+  assert(err == 0);
+
+  return sock;
+}
+
+
+int ipc_sendrecv_with_fds(bool send, int fd, void *buf, size_t buf_size, int* fds, int num_fds,
+                          int *out_num_fds) {
+  int err;
+
+  char control_buf[CMSG_SPACE(sizeof(int) * num_fds)];
+  memset(control_buf, 0, CMSG_SPACE(sizeof(int) * num_fds));
+
+  struct iovec iov = {
+    .iov_base = buf,
+    .iov_len = buf_size,
+  };
+  struct msghdr msg = {
+    .msg_iov = &iov,
+    .msg_iovlen = 1,
+  };
+
+  if (num_fds > 0) {
+    assert(fds);
+
+    msg.msg_control = control_buf;
+    msg.msg_controllen = CMSG_SPACE(sizeof(int) * num_fds);
+  }
+
+  if (send) {
+    if (num_fds) {
+      struct cmsghdr *cmsg = CMSG_FIRSTHDR(&msg);
+      assert(cmsg);
+      cmsg->cmsg_level = SOL_SOCKET;
+      cmsg->cmsg_type = SCM_RIGHTS;
+      cmsg->cmsg_len = CMSG_LEN(sizeof(int) * num_fds);
+      memcpy(CMSG_DATA(cmsg), fds, sizeof(int) * num_fds);
+      // printf("send clen %d -> %d\n", num_fds, cmsg->cmsg_len);
+    }
+    return sendmsg(fd, &msg, 0);
+  } else {
+    int r = recvmsg(fd, &msg, 0);
+    if (r < 0) return r;
+
+    int recv_fds = 0;
+    if (msg.msg_controllen > 0) {
+      struct cmsghdr *cmsg = CMSG_FIRSTHDR(&msg);
+      assert(cmsg);
+      assert(cmsg->cmsg_level == SOL_SOCKET && cmsg->cmsg_type == SCM_RIGHTS);
+      recv_fds = (cmsg->cmsg_len - CMSG_LEN(0));
+      assert(recv_fds > 0 && (recv_fds % sizeof(int)) == 0);
+      recv_fds /= sizeof(int);
+      // printf("recv clen %d -> %d\n", cmsg->cmsg_len, recv_fds);
+      // assert(cmsg->cmsg_len == CMSG_LEN(sizeof(int) * num_fds));
+
+      assert(fds && recv_fds <= num_fds);
+      memcpy(fds, CMSG_DATA(cmsg), sizeof(int) * recv_fds);
+    }
+
+    if (msg.msg_flags) {
+      for (int i=0; i<recv_fds; i++) {
+        close(fds[i]);
+      }
+      return -1;
+    }
+
+    if (fds) {
+      assert(out_num_fds);
+      *out_num_fds = recv_fds;
+    }
+    return r;
+  }
+}

selfdrive/common/ipc.h

@@ -0,0 +1,19 @@
+#ifndef IPC_H
+#define IPC_H
+
+#include <stdbool.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+int ipc_connect(const char* socket_path);
+int ipc_bind(const char* socket_path);
+int ipc_sendrecv_with_fds(bool send, int fd, void *buf, size_t buf_size, int* fds, int num_fds,
+                          int *out_num_fds);
+
+#ifdef __cplusplus
+}  // extern "C"
+#endif
+
+#endif

selfdrive/common/params.cc

@@ -1,4 +1,4 @@
-#include "selfdrive/common/params.h"
+#include "common/params.h"
 
 #ifndef _GNU_SOURCE
 #define _GNU_SOURCE
@@ -13,8 +13,8 @@
 #include <map>
 #include <string>
 
-#include "selfdrive/common/util.h"
+#include "common/util.h"
-#include "selfdrive/common/utilpp.h"
+#include "common/utilpp.h"
 
 namespace {
 

selfdrive/common/utilpp.h

@@ -9,10 +9,6 @@
 #include <sstream>
 #include <fstream>
 
-#ifdef __x86_64
-#include <linux/limits.h>
-#endif
-
 namespace util {
 
 inline bool starts_with(std::string s, std::string prefix) {
@@ -56,7 +52,7 @@ inline std::string dir_name(std::string const & path) {
 }
 
 inline std::string readlink(std::string path) {
-    char buff[PATH_MAX];
+  char buff[4096];
   ssize_t len = ::readlink(path.c_str(), buff, sizeof(buff)-1);
   if (len != -1) {
     buff[len] = '\0';

selfdrive/common/version.h

@@ -1 +1 @@
-#define COMMA_VERSION "0.5.3-release"
+#define COMMA_VERSION "0.5.4-release"

selfdrive/common/visionimg.cc

@@ -36,14 +36,17 @@ extern "C" void compute_aligned_width_and_height(int width,
                                       int *aligned_h);
 #endif
 
-VisionImg visionimg_alloc_rgb24(int width, int height, VisionBuf *out_buf) {
+void visionimg_compute_aligned_width_and_height(int width, int height, int *aligned_w, int *aligned_h) {
-
-  int aligned_w = 0, aligned_h = 0;
 #ifdef QCOM
-  compute_aligned_width_and_height(ALIGN(width, 32), ALIGN(height, 32), 3, 0, 0, 512, &aligned_w, &aligned_h);
+  compute_aligned_width_and_height(ALIGN(width, 32), ALIGN(height, 32), 3, 0, 0, 512, aligned_w, aligned_h);
 #else
-  aligned_w = width; aligned_h = height;
+  *aligned_w = width; *aligned_h = height;
 #endif
+}
+
+VisionImg visionimg_alloc_rgb24(int width, int height, VisionBuf *out_buf) {
+  int aligned_w = 0, aligned_h = 0;
+  visionimg_compute_aligned_width_and_height(width, height, &aligned_w, &aligned_h);
 
   int stride = aligned_w * 3;
   size_t size = aligned_w * aligned_h * 3;

selfdrive/common/visionimg.h

@@ -23,6 +23,7 @@ typedef struct VisionImg {
   size_t size;
 } VisionImg;
 
+void visionimg_compute_aligned_width_and_height(int width, int height, int *aligned_w, int *aligned_h);
 VisionImg visionimg_alloc_rgb24(int width, int height, VisionBuf *out_buf);
 
 #ifdef QCOM

selfdrive/common/visionipc.c

@@ -10,6 +10,8 @@
 #include <sys/socket.h>
 #include <sys/un.h>
 
+#include "ipc.h"
+
 #include "visionipc.h"
 
 typedef struct VisionPacketWire {
@@ -18,95 +20,14 @@ typedef struct VisionPacketWire {
 } VisionPacketWire;
 
 int vipc_connect() {
-  int err;
+  return ipc_connect(VIPC_SOCKET_PATH);
-
-  int sock = socket(AF_UNIX, SOCK_SEQPACKET, 0);
-  assert(sock >= 0);
-  struct sockaddr_un addr = {
-    .sun_family = AF_UNIX,
-    .sun_path = VIPC_SOCKET_PATH,
-  };
-  err = connect(sock, (struct sockaddr*)&addr, sizeof(addr));
-  if (err != 0) {
-    close(sock);
-    return -1;
-  }
-
-  return sock;
 }
 
-static int sendrecv_with_fds(bool send, int fd, void *buf, size_t buf_size, int* fds, int num_fds,
-                             int *out_num_fds) {
-  int err;
-
-  char control_buf[CMSG_SPACE(sizeof(int) * num_fds)];
-  memset(control_buf, 0, CMSG_SPACE(sizeof(int) * num_fds));
-
-  struct iovec iov = {
-    .iov_base = buf,
-    .iov_len = buf_size,
-  };
-  struct msghdr msg = {
-    .msg_iov = &iov,
-    .msg_iovlen = 1,
-  };
-
-  if (num_fds > 0) {
-    assert(fds);
-
-    msg.msg_control = control_buf;
-    msg.msg_controllen = CMSG_SPACE(sizeof(int) * num_fds);
-  }
-
-  if (send) {
-    if (num_fds) {
-      struct cmsghdr *cmsg = CMSG_FIRSTHDR(&msg);
-      assert(cmsg);
-      cmsg->cmsg_level = SOL_SOCKET;
-      cmsg->cmsg_type = SCM_RIGHTS;
-      cmsg->cmsg_len = CMSG_LEN(sizeof(int) * num_fds);
-      memcpy(CMSG_DATA(cmsg), fds, sizeof(int) * num_fds);
-      // printf("send clen %d -> %d\n", num_fds, cmsg->cmsg_len);
-    }
-    return sendmsg(fd, &msg, 0);
-  } else {
-    int r = recvmsg(fd, &msg, 0);
-    if (r < 0) return r;
-
-    int recv_fds = 0;
-    if (msg.msg_controllen > 0) {
-      struct cmsghdr *cmsg = CMSG_FIRSTHDR(&msg);
-      assert(cmsg);
-      assert(cmsg->cmsg_level == SOL_SOCKET && cmsg->cmsg_type == SCM_RIGHTS);
-      recv_fds = (cmsg->cmsg_len - CMSG_LEN(0));
-      assert(recv_fds > 0 && (recv_fds % sizeof(int)) == 0);
-      recv_fds /= sizeof(int);
-      // printf("recv clen %d -> %d\n", cmsg->cmsg_len, recv_fds);
-      // assert(cmsg->cmsg_len == CMSG_LEN(sizeof(int) * num_fds));
-
-      assert(fds && recv_fds <= num_fds);
-      memcpy(fds, CMSG_DATA(cmsg), sizeof(int) * recv_fds);
-    }
-
-    if (msg.msg_flags) {
-      for (int i=0; i<recv_fds; i++) {
-        close(fds[i]);
-      }
-      return -1;
-    }
-
-    if (fds) {
-      assert(out_num_fds);
-      *out_num_fds = recv_fds;
-    }
-    return r;
-  }
-}
 
 int vipc_recv(int fd, VisionPacket *out_p) {
   VisionPacketWire p = {0};
   VisionPacket p2 = {0};
-  int ret = sendrecv_with_fds(false, fd, &p, sizeof(p), (int*)p2.fds, VIPC_MAX_FDS, &p2.num_fds);
+  int ret = ipc_sendrecv_with_fds(false, fd, &p, sizeof(p), (int*)p2.fds, VIPC_MAX_FDS, &p2.num_fds);
   if (ret < 0) {
     printf("vipc_recv err: %s\n", strerror(errno));
   } else {
@@ -124,7 +45,7 @@ int vipc_send(int fd, const VisionPacket *p2) {
     .type = p2->type,
     .d = p2->d,
   };
-  return sendrecv_with_fds(true, fd, (void*)&p, sizeof(p), (int*)p2->fds, p2->num_fds, NULL);
+  return ipc_sendrecv_with_fds(true, fd, (void*)&p, sizeof(p), (int*)p2->fds, p2->num_fds, NULL);
 }
 
 void vipc_bufs_load(VIPCBuf *bufs, const VisionStreamBufs *stream_bufs,

selfdrive/controls/controlsd.py

@@ -46,8 +46,8 @@ def isEnabled(state):
 
 
 def data_sample(CI, CC, thermal, calibration, health, driver_monitor, gps_location,
-                poller, cal_status, overtemp, free_space, driver_status, geofence,
+                poller, cal_status, cal_perc, overtemp, free_space, low_battery,
-                state, mismatch_counter, params):
+                driver_status, geofence, state, mismatch_counter, params):
 
   # *** read can and compute car states ***
   CS = CI.update(CC)
@@ -80,6 +80,12 @@ def data_sample(CI, CC, thermal, calibration, health, driver_monitor, gps_locati
     # under 15% of space free no enable allowed
     free_space = td.thermal.freeSpace < 0.15
 
+    # at zero percent battery, OP should not be allowed
+    low_battery = td.thermal.batteryPercent < 1
+
+  if low_battery:
+    events.append(create_event('lowBattery', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
+
   if overtemp:
     events.append(create_event('overheat', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
 
@@ -89,10 +95,11 @@ def data_sample(CI, CC, thermal, calibration, health, driver_monitor, gps_locati
   # *** read calibration status ***
   if cal is not None:
     cal_status = cal.liveCalibration.calStatus
+    cal_perc = cal.liveCalibration.calPerc
 
   if cal_status != Calibration.CALIBRATED:
     if cal_status == Calibration.UNCALIBRATED:
-      events.append(create_event('calibrationInProgress', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
+      events.append(create_event('calibrationIncomplete', [ET.NO_ENTRY, ET.SOFT_DISABLE, ET.PERMANENT]))
     else:
       events.append(create_event('calibrationInvalid', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
 
@@ -117,7 +124,7 @@ def data_sample(CI, CC, thermal, calibration, health, driver_monitor, gps_locati
   if geofence is not None and not geofence.in_geofence:
     events.append(create_event('geofence', [ET.NO_ENTRY, ET.WARNING]))
 
-  return CS, events, cal_status, overtemp, free_space, mismatch_counter
+  return CS, events, cal_status, cal_perc, overtemp, free_space, low_battery, mismatch_counter
 
 
 def calc_plan(CS, CP, events, PL, LaC, LoC, v_cruise_kph, driver_status, geofence):
@@ -224,7 +231,7 @@ def state_transition(CS, CP, state, events, soft_disable_timer, v_cruise_kph, AM
 
 
 def state_control(plan, CS, CP, state, events, v_cruise_kph, v_cruise_kph_last, AM, rk,
-                  driver_status, PL, LaC, LoC, VM, angle_offset, passive, is_metric):
+                  driver_status, PL, LaC, LoC, VM, angle_offset, passive, is_metric, cal_perc):
   # Given the state, this function returns the actuators
 
   # reset actuators to zero
@@ -258,13 +265,13 @@ def state_control(plan, CS, CP, state, events, v_cruise_kph, v_cruise_kph_last,
 
     # parse warnings from car specific interface
     for e in get_events(events, [ET.WARNING]):
-      extra_text = ''
+      extra_text = ""
       if e == "belowSteerSpeed":
         if is_metric:
           extra_text = str(int(round(CP.minSteerSpeed * CV.MS_TO_KPH))) + " kph"
         else:
           extra_text = str(int(round(CP.minSteerSpeed * CV.MS_TO_MPH))) + " mph"
-      AM.add(e, enabled, extra_text=extra_text)
+      AM.add(e, enabled, extra_text_2=extra_text)
 
   # *** angle offset learning ***
 
@@ -289,10 +296,13 @@ def state_control(plan, CS, CP, state, events, v_cruise_kph, v_cruise_kph_last,
 
   # parse permanent warnings to display constantly
   for e in get_events(events, [ET.PERMANENT]):
-    AM.add(str(e) + "Permanent", enabled)
+    extra_text_1, extra_text_2 = "", ""
+    if e == "calibrationIncomplete":
+      extra_text_1 = str(cal_perc) + "%"
+      extra_text_2 = "35 kph" if is_metric else "15 mph"
+    AM.add(str(e) + "Permanent", enabled, extra_text_1=extra_text_1, extra_text_2=extra_text_2)
 
   # *** process alerts ***
-
   AM.process_alerts(sec_since_boot())
 
   return actuators, v_cruise_kph, driver_status, angle_offset
@@ -475,8 +485,10 @@ def controlsd_thread(gctx=None, rate=100, default_bias=0.):
   v_cruise_kph_last = 0
   overtemp = False
   free_space = False
-  cal_status = Calibration.UNCALIBRATED
+  cal_status = Calibration.INVALID
+  cal_perc = 0
   mismatch_counter = 0
+  low_battery = False
 
   rk = Ratekeeper(rate, print_delay_threshold=2./1000)
 
@@ -497,8 +509,8 @@ def controlsd_thread(gctx=None, rate=100, default_bias=0.):
     prof.checkpoint("Ratekeeper", ignore=True)
 
     # sample data and compute car events
-    CS, events, cal_status, overtemp, free_space, mismatch_counter = data_sample(CI, CC, thermal, cal, health,
+    CS, events, cal_status, cal_perc, overtemp, free_space, low_battery, mismatch_counter = data_sample(CI, CC, thermal, cal, health,
-      driver_monitor, gps_location, poller, cal_status, overtemp, free_space, driver_status, geofence, state, mismatch_counter, params)
+      driver_monitor, gps_location, poller, cal_status, cal_perc, overtemp, free_space, low_battery, driver_status, geofence, state, mismatch_counter, params)
     prof.checkpoint("Sample")
 
     # define plan
@@ -513,7 +525,7 @@ def controlsd_thread(gctx=None, rate=100, default_bias=0.):
 
     # compute actuators
     actuators, v_cruise_kph, driver_status, angle_offset = state_control(plan, CS, CP, state, events, v_cruise_kph,
-      v_cruise_kph_last, AM, rk, driver_status, PL, LaC, LoC, VM, angle_offset, passive, is_metric)
+      v_cruise_kph_last, AM, rk, driver_status, PL, LaC, LoC, VM, angle_offset, passive, is_metric, cal_perc)
     prof.checkpoint("State Control")
 
     # publish data

selfdrive/controls/lib/alertmanager.py

@@ -6,11 +6,12 @@ import copy
 
 # Priority
 class Priority:
-  HIGHEST = 4
-  HIGH = 3
-  MID = 2
-  LOW = 1
   LOWEST = 0
+  LOW_LOWEST = 1
+  LOW = 2
+  MID = 3
+  HIGH = 4
+  HIGHEST = 5
 
 AlertSize = log.Live100Data.AlertSize
 AlertStatus = log.Live100Data.AlertStatus
@@ -155,7 +156,7 @@ class AlertManager(object):
         "Be ready to take over at any time",
         "Always keep hands on wheel and eyes on road",
         AlertStatus.normal, AlertSize.mid,
-        Priority.LOWEST, None, None, 0., 0., 15.),
+        Priority.LOW_LOWEST, None, None, 0., 0., 15.),
 
     "ethicalDilemma": Alert(
         "TAKE CONTROL IMMEDIATELY",
@@ -242,6 +243,12 @@ class AlertManager(object):
         AlertStatus.normal, AlertSize.mid,
         Priority.LOW, None, "chimeDouble", .4, 2., 3.),
 
+    "lowBatteryNoEntry": Alert(
+        "openpilot Unavailable",
+        "Low Battery",
+        AlertStatus.normal, AlertSize.mid,
+        Priority.LOW, None, "chimeDouble", .4, 2., 3.),
+
     # Cancellation alerts causing soft disabling
     "overheat": Alert(
         "TAKE CONTROL IMMEDIATELY",
@@ -261,7 +268,7 @@ class AlertManager(object):
         AlertStatus.critical, AlertSize.full,
         Priority.MID, "steerRequired", "chimeRepeated", 1., 3., 3.),
 
-    "calibrationInProgress": Alert(
+    "calibrationIncomplete": Alert(
         "TAKE CONTROL IMMEDIATELY",
         "Calibration in Progress",
         AlertStatus.critical, AlertSize.full,
@@ -285,6 +292,12 @@ class AlertManager(object):
         AlertStatus.critical, AlertSize.full,
         Priority.MID, "steerRequired", "chimeRepeated", 1., 3., 3.),
 
+    "lowBattery": Alert(
+        "TAKE CONTROL IMMEDIATELY",
+        "Low Battery",
+        AlertStatus.critical, AlertSize.full,
+        Priority.MID, "steerRequired", "chimeRepeated", 1., 3., 3.),
+
     # Cancellation alerts causing immediate disabling
     "radarCommIssue": Alert(
         "TAKE CONTROL IMMEDIATELY",
@@ -324,13 +337,13 @@ class AlertManager(object):
 
     "steerUnavailable": Alert(
         "TAKE CONTROL IMMEDIATELY",
-        "Steer Fault: Restart the Car",
+        "LKAS Fault: Restart the Car",
         AlertStatus.critical, AlertSize.full,
         Priority.HIGHEST, "steerRequired", "chimeRepeated", 1., 3., 4.),
 
     "brakeUnavailable": Alert(
         "TAKE CONTROL IMMEDIATELY",
-        "Brake Fault: Restart the Car",
+        "Cruise Fault: Restart the Car",
         AlertStatus.critical, AlertSize.full,
         Priority.HIGHEST, "steerRequired", "chimeRepeated", 1., 3., 4.),
 
@@ -390,7 +403,7 @@ class AlertManager(object):
         AlertStatus.normal, AlertSize.mid,
         Priority.LOW, None, "chimeDouble", .4, 2., 3.),
 
-    "calibrationInProgressNoEntry": Alert(
+    "calibrationIncompleteNoEntry": Alert(
         "openpilot Unavailable",
         "Calibration in Progress",
         AlertStatus.normal, AlertSize.mid,
@@ -452,13 +465,13 @@ class AlertManager(object):
 
     "steerUnavailableNoEntry": Alert(
         "openpilot Unavailable",
-        "Steer Fault: Restart the Car",
+        "LKAS Fault: Restart the Car",
         AlertStatus.normal, AlertSize.mid,
         Priority.LOW, None, "chimeDouble", .4, 2., 3.),
 
     "brakeUnavailableNoEntry": Alert(
         "openpilot Unavailable",
-        "Brake Fault: Restart the Car",
+        "Cruise Fault: Restart the Car",
         AlertStatus.normal, AlertSize.mid,
         Priority.LOW, None, "chimeDouble", .4, 2., 3.),
 
@@ -492,23 +505,29 @@ class AlertManager(object):
         AlertStatus.normal, AlertSize.mid,
         Priority.LOW, None, "chimeDouble", .4, 2., 3.),
 
-    # permanent alerts to display on small UI upper box
+    # permanent alerts
     "steerUnavailablePermanent": Alert(
-        "STEER FAULT: Restart the car to engage",
+        "LKAS Fault: Restart the car to engage",
         "",
         AlertStatus.normal, AlertSize.small,
-        Priority.LOWEST, None, None, 0., 0., .2),
+        Priority.LOW_LOWEST, None, None, 0., 0., .2),
 
     "brakeUnavailablePermanent": Alert(
-        "BRAKE FAULT: Restart the car to engage",
+        "Cruise Fault: Restart the car to engage",
         "",
         AlertStatus.normal, AlertSize.small,
-        Priority.LOWEST, None, None, 0., 0., .2),
+        Priority.LOW_LOWEST, None, None, 0., 0., .2),
 
     "lowSpeedLockoutPermanent": Alert(
-        "CRUISE FAULT: Restart the car to engage",
+        "Cruise Fault: Restart the car to engage",
         "",
         AlertStatus.normal, AlertSize.small,
+        Priority.LOW_LOWEST, None, None, 0., 0., .2),
+
+    "calibrationIncompletePermanent": Alert(
+        "Calibration in Progress: ",
+        "Drive Above ",
+        AlertStatus.normal, AlertSize.mid,
         Priority.LOWEST, None, None, 0., 0., .2),
   }
 
@@ -518,10 +537,11 @@ class AlertManager(object):
   def alertPresent(self):
     return len(self.activealerts) > 0
 
-  def add(self, alert_type, enabled=True, extra_text=''):
+  def add(self, alert_type, enabled=True, extra_text_1='', extra_text_2=''):
     alert_type = str(alert_type)
     added_alert = copy.copy(self.alerts[alert_type])
-    added_alert.alert_text_2 += extra_text
+    added_alert.alert_text_1 += extra_text_1
+    added_alert.alert_text_2 += extra_text_2
     added_alert.start_time = sec_since_boot()
 
     # if new alert is higher priority, log it

selfdrive/controls/lib/driver_monitor.py

@@ -1,12 +1,14 @@
 import numpy as np
 from common.realtime import sec_since_boot
 from selfdrive.controls.lib.drive_helpers import create_event, EventTypes as ET
+from common.filter_simple import FirstOrderFilter
 
 _DT = 0.01                  # update runs at 100Hz
+_DTM = 0.1                  # DM runs at 10Hz
 _AWARENESS_TIME = 180       # 3 minutes limit without user touching steering wheels make the car enter a terminal status
 _AWARENESS_PRE_TIME = 20.   # a first alert is issued 20s before expiration
 _AWARENESS_PROMPT_TIME = 5. # a second alert is issued 5s before start decelerating the car
-_DISTRACTED_TIME = 8.
+_DISTRACTED_TIME = 7.
 _DISTRACTED_PRE_TIME = 4.
 _DISTRACTED_PROMPT_TIME = 2.
 # measured 1 rad in x FOV. 1152x864 is original image, 160x320 is a right crop for model
@@ -18,13 +20,10 @@ _CAMERA_X_CONV = 0.375      # 160*864/320/1152
 _PITCH_WEIGHT = 1.5  # pitch matters a lot more
 _METRIC_THRESHOLD = 0.4
 _PITCH_POS_ALLOWANCE = 0.08  # rad, to not be too sensitive on positive pitch
-_DTM = 0.1                   # driver monitor runs at 10Hz
 _PITCH_NATURAL_OFFSET = 0.1  # people don't seem to look straight when they drive relaxed, rather a bit up
 _STD_THRESHOLD = 0.1         # above this standard deviation consider the measurement invalid
-_DISTRACTED_FILTER_F = 0.6   # 0.6Hz, 0.25s ts
+_DISTRACTED_FILTER_TS = 0.25 # 0.6Hz
-_DISTRACTED_FILTER_K = 2 * np.pi * _DISTRACTED_FILTER_F * _DTM / (1 + 2 * np.pi * _DISTRACTED_FILTER_F * _DTM)
+_VARIANCE_FILTER_TS = 20.    # 0.008Hz
-_VARIANCE_FILTER_F = 0.008    # 0.008Hz, 20s ts
-_VARIANCE_FILTER_K = 2 * np.pi * _VARIANCE_FILTER_F * _DTM / (1 + 2 * np.pi * _VARIANCE_FILTER_F * _DTM)
 
 
 class _DriverPose():
@@ -47,15 +46,15 @@ class DriverStatus():
     self.monitor_valid = True   # variance needs to be low
     self.awareness = 1.
     self.driver_distracted = False
-    self.driver_distraction_level = 0.
+    self.driver_distraction_filter = FirstOrderFilter(0., _DISTRACTED_FILTER_TS, _DTM)
     self.variance_high = False
-    self.variance_level = 0.
+    self.variance_filter = FirstOrderFilter(0., _VARIANCE_FILTER_TS, _DTM)
     self.ts_last_check = 0.
     self._set_timers()
 
   def _reset_filters(self):
-    self.driver_distraction_level = 0.
+    self.driver_distraction_filter.x = 0.
-    self.variance_level = 0.
+    self.variance_filter.x = 0.
     self.monitor_valid = True
 
   def _set_timers(self):
@@ -90,11 +89,9 @@ class DriverStatus():
     self.pose.pitch_offset = -driver_monitoring.descriptor[4] * _CAMERA_FOV_Y  # positive y is down
     self.driver_distracted = self._is_driver_distracted(self.pose)
     # first order filters
-    self.driver_distraction_level = (1. - _DISTRACTED_FILTER_K) * self.driver_distraction_level + \
+    self.driver_distraction_filter.update(self.driver_distracted)
-                                    _DISTRACTED_FILTER_K * self.driver_distracted
     self.variance_high = driver_monitoring.std > _STD_THRESHOLD
-    self.variance_level = (1. - _VARIANCE_FILTER_K) * self.variance_level + \
+    self.variance_filter.update(self.variance_high)
-                          _VARIANCE_FILTER_K * self.variance_high
 
     monitor_param_on_prev = self.monitor_param_on
     monitor_valid_prev = self.monitor_valid
@@ -105,7 +102,7 @@ class DriverStatus():
       self.monitor_param_on = params.get("IsDriverMonitoringEnabled") == "1"
       self.ts_last_check = ts
 
-    self.monitor_valid = _monitor_hysteresys(self.variance_level, monitor_valid_prev)
+    self.monitor_valid = _monitor_hysteresys(self.variance_filter.x, monitor_valid_prev)
     self.monitor_on = self.monitor_valid and self.monitor_param_on
     if monitor_param_on_prev != self.monitor_param_on:
       self._reset_filters()
@@ -114,13 +111,13 @@ class DriverStatus():
 
   def update(self, events, driver_engaged, ctrl_active, standstill):
 
-    driver_engaged |= (self.driver_distraction_level < 0.37 and self.monitor_on)
+    driver_engaged |= (self.driver_distraction_filter.x < 0.37 and self.monitor_on)
 
     if (driver_engaged and self.awareness > 0.) or not ctrl_active:
       # always reset if driver is in control (unless we are in red alert state) or op isn't active
       self.awareness = 1.
 
-    if (not self.monitor_on or (self.driver_distraction_level > 0.63 and self.driver_distracted)) and \
+    if (not self.monitor_on or (self.driver_distraction_filter.x > 0.63 and self.driver_distracted)) and \
        not (standstill and self.awareness - self.step_change <= self.threshold_prompt):
       self.awareness = max(self.awareness - self.step_change, -0.1)
 
@@ -142,11 +139,11 @@ class DriverStatus():
 
 if __name__ == "__main__":
   ds = DriverStatus(True)
-  ds.driver_distraction_level = 1.
+  ds.driver_distraction_filter.x = 0.
   ds.driver_distracted = 1
-  for i in range(1000):
+  for i in range(10):
-    ds.update([], False, True, True)
+    ds.update([], False, True, False)
-    print(ds.awareness, ds.driver_distracted, ds.driver_distraction_level)
+    print(ds.awareness, ds.driver_distracted, ds.driver_distraction_filter.x)
   ds.update([], True, True, False)
-  print(ds.awareness, ds.driver_distracted, ds.driver_distraction_level)
+  print(ds.awareness, ds.driver_distracted, ds.driver_distraction_filter.x)
 

selfdrive/controls/lib/lateral_mpc/Makefile

@@ -20,58 +20,49 @@ ACADO_LIBS := -L $(PHONELIBS)/acado/x64/lib -l:libacado_toolkit.a -l:libacado_ca
 endif
 
 OBJS = \
-  qp/Bounds.o \
+  lib_qp/Bounds.o \
-  qp/Constraints.o \
+  lib_qp/Constraints.o \
-  qp/CyclingManager.o \
+  lib_qp/CyclingManager.o \
-  qp/Indexlist.o \
+  lib_qp/Indexlist.o \
-  qp/MessageHandling.o \
+  lib_qp/MessageHandling.o \
-  qp/QProblem.o \
+  lib_qp/QProblem.o \
-  qp/QProblemB.o \
+  lib_qp/QProblemB.o \
-  qp/SubjectTo.o \
+  lib_qp/SubjectTo.o \
-  qp/Utils.o \
+  lib_qp/Utils.o \
-  qp/EXTRAS/SolutionAnalysis.o \
+  lib_qp/EXTRAS/SolutionAnalysis.o \
-	mpc_export/acado_qpoases_interface.o \
+	lib_mpc_export/acado_qpoases_interface.o \
-  mpc_export/acado_integrator.o \
+  lib_mpc_export/acado_integrator.o \
-  mpc_export/acado_solver.o \
+  lib_mpc_export/acado_solver.o \
-  mpc_export/acado_auxiliary_functions.o \
+  lib_mpc_export/acado_auxiliary_functions.o \
-  mpc.o
+  lateral_mpc.o
 
 DEPS := $(OBJS:.o=.d)
 
 .PHONY: all
-all: libcommampc.so
+all: libmpc.so
 
-libcommampc.so: $(OBJS)
+libmpc.so: $(OBJS)
 	$(CXX) -shared -o '$@' $^ -lm
 
-
+lib_qp/%.o: $(PHONELIBS)/qpoases/SRC/%.cpp
-qp/%.o: $(PHONELIBS)/qpoases/SRC/%.cpp
-	@echo "[ CXX ] $@"
-	mkdir -p qp
-	$(CXX) $(CXXFLAGS) -MMD \
-           -I mpc_export/ \
-           $(QPOASES_FLAGS) \
-           -c -o '$@' '$<'
-
-qp/EXTRAS/%.o: $(PHONELIBS)/qpoases/SRC/EXTRAS/%.cpp
 	@echo "[ CXX ] $@"
-	mkdir -p qp/EXTRAS
+	mkdir -p lib_qp/EXTRAS
 	$(CXX) $(CXXFLAGS) -MMD \
-           -I mpc_export/ \
+           -I lib_mpc_export/ \
            $(QPOASES_FLAGS) \
            -c -o '$@' '$<'
 
 %.o: %.cpp
 	@echo "[ CXX ] $@"
 	$(CXX) $(CXXFLAGS) -MMD \
-           -I mpc_export/ \
+           -I lib_mpc_export/ \
            $(QPOASES_FLAGS) \
            -c -o '$@' '$<'
 
 %.o: %.c
 	@echo "[ CC ] $@"
 	$(CC) $(CFLAGS) -MMD \
-           -I mpc_export/ \
+           -I lib_mpc_export/ \
            $(QPOASES_FLAGS) \
            -c -o '$@' '$<'
 
@@ -88,6 +79,6 @@ generate: generator
 
 .PHONY: clean
 clean:
-	rm -f libcommampc.so generator $(OBJS) $(DEPS)
+	rm -f *.so generator $(OBJS) $(DEPS)
 
 -include $(DEPS)

selfdrive/controls/lib/lateral_mpc/generator.cpp

@@ -141,7 +141,7 @@ int main( )
   mpc.set( GENERATE_MATLAB_INTERFACE, NO );
   mpc.set( GENERATE_SIMULINK_INTERFACE, NO );
 
-  if (mpc.exportCode( "mpc_export" ) != SUCCESSFUL_RETURN)
+  if (mpc.exportCode( "lib_mpc_export" ) != SUCCESSFUL_RETURN)
     exit( EXIT_FAILURE );
 
   mpc.printDimensionsQP( );

selfdrive/controls/lib/lateral_mpc/lateral_mpc.d

@@ -0,0 +1,3 @@
+lateral_mpc.o: lateral_mpc.c lib_mpc_export/acado_common.h \
+  lib_mpc_export/acado_qpoases_interface.hpp \
+  lib_mpc_export/acado_auxiliary_functions.h

selfdrive/controls/lib/lateral_mpc/lateral_mpc.o

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

selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_auxiliary_functions.d

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

selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_auxiliary_functions.o

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

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

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

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

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

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

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

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

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

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

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

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

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

selfdrive/controls/lib/lateral_mpc/lib_qp/Bounds.d

@@ -0,0 +1,14 @@
+lib_qp/Bounds.o: ../../../../phonelibs/qpoases/SRC/Bounds.cpp \
+  ../../../../phonelibs/qpoases/INCLUDE/Bounds.hpp \
+  ../../../../phonelibs/qpoases/INCLUDE/SubjectTo.hpp \
+  ../../../../phonelibs/qpoases/INCLUDE/Indexlist.hpp \
+  ../../../../phonelibs/qpoases/INCLUDE/Utils.hpp \
+  ../../../../phonelibs/qpoases/INCLUDE/MessageHandling.hpp \
+  ../../../../phonelibs/qpoases/INCLUDE/Types.hpp \
+  ../../../../phonelibs/qpoases/INCLUDE/Constants.hpp \
+  lib_mpc_export/acado_qpoases_interface.hpp \
+  ../../../../phonelibs/qpoases/SRC/MessageHandling.ipp \
+  ../../../../phonelibs/qpoases/SRC/Utils.ipp \
+  ../../../../phonelibs/qpoases/SRC/Indexlist.ipp \
+  ../../../../phonelibs/qpoases/SRC/SubjectTo.ipp \
+  ../../../../phonelibs/qpoases/SRC/Bounds.ipp

selfdrive/controls/lib/lateral_mpc/lib_qp/Bounds.o

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

selfdrive/controls/lib/lateral_mpc/lib_qp/Constraints.d

@@ -0,0 +1,14 @@
+lib_qp/Constraints.o: ../../../../phonelibs/qpoases/SRC/Constraints.cpp \
+  ../../../../phonelibs/qpoases/INCLUDE/Constraints.hpp \
+  ../../../../phonelibs/qpoases/INCLUDE/SubjectTo.hpp \
+  ../../../../phonelibs/qpoases/INCLUDE/Indexlist.hpp \
+  ../../../../phonelibs/qpoases/INCLUDE/Utils.hpp \
+  ../../../../phonelibs/qpoases/INCLUDE/MessageHandling.hpp \
+  ../../../../phonelibs/qpoases/INCLUDE/Types.hpp \
+  ../../../../phonelibs/qpoases/INCLUDE/Constants.hpp \
+  lib_mpc_export/acado_qpoases_interface.hpp \
+  ../../../../phonelibs/qpoases/SRC/MessageHandling.ipp \
+  ../../../../phonelibs/qpoases/SRC/Utils.ipp \
+  ../../../../phonelibs/qpoases/SRC/Indexlist.ipp \
+  ../../../../phonelibs/qpoases/SRC/SubjectTo.ipp \
+  ../../../../phonelibs/qpoases/SRC/Constraints.ipp

selfdrive/controls/lib/lateral_mpc/lib_qp/Constraints.o

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

selfdrive/controls/lib/lateral_mpc/lib_qp/CyclingManager.d

@@ -0,0 +1,11 @@
+lib_qp/CyclingManager.o: \
+  ../../../../phonelibs/qpoases/SRC/CyclingManager.cpp \
+  ../../../../phonelibs/qpoases/INCLUDE/CyclingManager.hpp \
+  ../../../../phonelibs/qpoases/INCLUDE/Utils.hpp \
+  ../../../../phonelibs/qpoases/INCLUDE/MessageHandling.hpp \
+  ../../../../phonelibs/qpoases/INCLUDE/Types.hpp \
+  ../../../../phonelibs/qpoases/INCLUDE/Constants.hpp \
+  lib_mpc_export/acado_qpoases_interface.hpp \
+  ../../../../phonelibs/qpoases/SRC/MessageHandling.ipp \
+  ../../../../phonelibs/qpoases/SRC/Utils.ipp \
+  ../../../../phonelibs/qpoases/SRC/CyclingManager.ipp

selfdrive/controls/lib/lateral_mpc/lib_qp/CyclingManager.o

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

selfdrive/controls/lib/lateral_mpc/lib_qp/EXTRAS/SolutionAnalysis.d

@@ -0,0 +1,24 @@
+lib_qp/EXTRAS/SolutionAnalysis.o: \
+  ../../../../phonelibs/qpoases/SRC/EXTRAS/SolutionAnalysis.cpp \
+  ../../../../phonelibs/qpoases/INCLUDE/EXTRAS/SolutionAnalysis.hpp \
+  ../../../../phonelibs/qpoases/INCLUDE/QProblem.hpp \
+  ../../../../phonelibs/qpoases/INCLUDE/QProblemB.hpp \
+  ../../../../phonelibs/qpoases/INCLUDE/Bounds.hpp \
+  ../../../../phonelibs/qpoases/INCLUDE/SubjectTo.hpp \
+  ../../../../phonelibs/qpoases/INCLUDE/Indexlist.hpp \
+  ../../../../phonelibs/qpoases/INCLUDE/Utils.hpp \
+  ../../../../phonelibs/qpoases/INCLUDE/MessageHandling.hpp \
+  ../../../../phonelibs/qpoases/INCLUDE/Types.hpp \
+  ../../../../phonelibs/qpoases/INCLUDE/Constants.hpp \
+  lib_mpc_export/acado_qpoases_interface.hpp \
+  ../../../../phonelibs/qpoases/SRC/MessageHandling.ipp \
+  ../../../../phonelibs/qpoases/SRC/Utils.ipp \
+  ../../../../phonelibs/qpoases/SRC/Indexlist.ipp \
+  ../../../../phonelibs/qpoases/SRC/SubjectTo.ipp \
+  ../../../../phonelibs/qpoases/SRC/Bounds.ipp \
+  ../../../../phonelibs/qpoases/SRC/QProblemB.ipp \
+  ../../../../phonelibs/qpoases/INCLUDE/Constraints.hpp \
+  ../../../../phonelibs/qpoases/SRC/Constraints.ipp \
+  ../../../../phonelibs/qpoases/INCLUDE/CyclingManager.hpp \
+  ../../../../phonelibs/qpoases/SRC/CyclingManager.ipp \
+  ../../../../phonelibs/qpoases/SRC/QProblem.ipp

selfdrive/controls/lib/lateral_mpc/lib_qp/EXTRAS/SolutionAnalysis.o

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

selfdrive/controls/lib/lateral_mpc/lib_qp/Indexlist.d

@@ -0,0 +1,10 @@
+lib_qp/Indexlist.o: ../../../../phonelibs/qpoases/SRC/Indexlist.cpp \
+  ../../../../phonelibs/qpoases/INCLUDE/Indexlist.hpp \
+  ../../../../phonelibs/qpoases/INCLUDE/Utils.hpp \
+  ../../../../phonelibs/qpoases/INCLUDE/MessageHandling.hpp \
+  ../../../../phonelibs/qpoases/INCLUDE/Types.hpp \
+  ../../../../phonelibs/qpoases/INCLUDE/Constants.hpp \
+  lib_mpc_export/acado_qpoases_interface.hpp \
+  ../../../../phonelibs/qpoases/SRC/MessageHandling.ipp \
+  ../../../../phonelibs/qpoases/SRC/Utils.ipp \
+  ../../../../phonelibs/qpoases/SRC/Indexlist.ipp

selfdrive/controls/lib/lateral_mpc/lib_qp/Indexlist.o

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

selfdrive/controls/lib/lateral_mpc/lib_qp/MessageHandling.d

@@ -0,0 +1,9 @@
+lib_qp/MessageHandling.o: \
+  ../../../../phonelibs/qpoases/SRC/MessageHandling.cpp \
+  ../../../../phonelibs/qpoases/INCLUDE/MessageHandling.hpp \
+  ../../../../phonelibs/qpoases/INCLUDE/Types.hpp \
+  ../../../../phonelibs/qpoases/INCLUDE/Constants.hpp \
+  lib_mpc_export/acado_qpoases_interface.hpp \
+  ../../../../phonelibs/qpoases/SRC/MessageHandling.ipp \
+  ../../../../phonelibs/qpoases/INCLUDE/Utils.hpp \
+  ../../../../phonelibs/qpoases/SRC/Utils.ipp

selfdrive/controls/lib/lateral_mpc/lib_qp/MessageHandling.o

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

selfdrive/controls/lib/lateral_mpc/lib_qp/QProblem.d

@@ -0,0 +1,22 @@
+lib_qp/QProblem.o: ../../../../phonelibs/qpoases/SRC/QProblem.cpp \
+  ../../../../phonelibs/qpoases/INCLUDE/QProblem.hpp \
+  ../../../../phonelibs/qpoases/INCLUDE/QProblemB.hpp \
+  ../../../../phonelibs/qpoases/INCLUDE/Bounds.hpp \
+  ../../../../phonelibs/qpoases/INCLUDE/SubjectTo.hpp \
+  ../../../../phonelibs/qpoases/INCLUDE/Indexlist.hpp \
+  ../../../../phonelibs/qpoases/INCLUDE/Utils.hpp \
+  ../../../../phonelibs/qpoases/INCLUDE/MessageHandling.hpp \
+  ../../../../phonelibs/qpoases/INCLUDE/Types.hpp \
+  ../../../../phonelibs/qpoases/INCLUDE/Constants.hpp \
+  lib_mpc_export/acado_qpoases_interface.hpp \
+  ../../../../phonelibs/qpoases/SRC/MessageHandling.ipp \
+  ../../../../phonelibs/qpoases/SRC/Utils.ipp \
+  ../../../../phonelibs/qpoases/SRC/Indexlist.ipp \
+  ../../../../phonelibs/qpoases/SRC/SubjectTo.ipp \
+  ../../../../phonelibs/qpoases/SRC/Bounds.ipp \
+  ../../../../phonelibs/qpoases/SRC/QProblemB.ipp \
+  ../../../../phonelibs/qpoases/INCLUDE/Constraints.hpp \
+  ../../../../phonelibs/qpoases/SRC/Constraints.ipp \
+  ../../../../phonelibs/qpoases/INCLUDE/CyclingManager.hpp \
+  ../../../../phonelibs/qpoases/SRC/CyclingManager.ipp \
+  ../../../../phonelibs/qpoases/SRC/QProblem.ipp

selfdrive/controls/lib/lateral_mpc/lib_qp/QProblem.o

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

selfdrive/controls/lib/lateral_mpc/lib_qp/QProblemB.d

@@ -0,0 +1,16 @@
+lib_qp/QProblemB.o: ../../../../phonelibs/qpoases/SRC/QProblemB.cpp \
+  ../../../../phonelibs/qpoases/INCLUDE/QProblemB.hpp \
+  ../../../../phonelibs/qpoases/INCLUDE/Bounds.hpp \
+  ../../../../phonelibs/qpoases/INCLUDE/SubjectTo.hpp \
+  ../../../../phonelibs/qpoases/INCLUDE/Indexlist.hpp \
+  ../../../../phonelibs/qpoases/INCLUDE/Utils.hpp \
+  ../../../../phonelibs/qpoases/INCLUDE/MessageHandling.hpp \
+  ../../../../phonelibs/qpoases/INCLUDE/Types.hpp \
+  ../../../../phonelibs/qpoases/INCLUDE/Constants.hpp \
+  lib_mpc_export/acado_qpoases_interface.hpp \
+  ../../../../phonelibs/qpoases/SRC/MessageHandling.ipp \
+  ../../../../phonelibs/qpoases/SRC/Utils.ipp \
+  ../../../../phonelibs/qpoases/SRC/Indexlist.ipp \
+  ../../../../phonelibs/qpoases/SRC/SubjectTo.ipp \
+  ../../../../phonelibs/qpoases/SRC/Bounds.ipp \
+  ../../../../phonelibs/qpoases/SRC/QProblemB.ipp

selfdrive/controls/lib/lateral_mpc/lib_qp/QProblemB.o

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

selfdrive/controls/lib/lateral_mpc/lib_qp/SubjectTo.d

@@ -0,0 +1,12 @@
+lib_qp/SubjectTo.o: ../../../../phonelibs/qpoases/SRC/SubjectTo.cpp \
+  ../../../../phonelibs/qpoases/INCLUDE/SubjectTo.hpp \
+  ../../../../phonelibs/qpoases/INCLUDE/Indexlist.hpp \
+  ../../../../phonelibs/qpoases/INCLUDE/Utils.hpp \
+  ../../../../phonelibs/qpoases/INCLUDE/MessageHandling.hpp \
+  ../../../../phonelibs/qpoases/INCLUDE/Types.hpp \
+  ../../../../phonelibs/qpoases/INCLUDE/Constants.hpp \
+  lib_mpc_export/acado_qpoases_interface.hpp \
+  ../../../../phonelibs/qpoases/SRC/MessageHandling.ipp \
+  ../../../../phonelibs/qpoases/SRC/Utils.ipp \
+  ../../../../phonelibs/qpoases/SRC/Indexlist.ipp \
+  ../../../../phonelibs/qpoases/SRC/SubjectTo.ipp

selfdrive/controls/lib/lateral_mpc/lib_qp/SubjectTo.o

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

selfdrive/controls/lib/lateral_mpc/lib_qp/Utils.d

@@ -0,0 +1,8 @@
+lib_qp/Utils.o: ../../../../phonelibs/qpoases/SRC/Utils.cpp \
+  ../../../../phonelibs/qpoases/INCLUDE/Utils.hpp \
+  ../../../../phonelibs/qpoases/INCLUDE/MessageHandling.hpp \
+  ../../../../phonelibs/qpoases/INCLUDE/Types.hpp \
+  ../../../../phonelibs/qpoases/INCLUDE/Constants.hpp \
+  lib_mpc_export/acado_qpoases_interface.hpp \
+  ../../../../phonelibs/qpoases/SRC/MessageHandling.ipp \
+  ../../../../phonelibs/qpoases/SRC/Utils.ipp

selfdrive/controls/lib/lateral_mpc/lib_qp/Utils.o

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

selfdrive/controls/lib/lateral_mpc/libmpc.so

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

selfdrive/controls/lib/lateral_mpc/libmpc_py.py

@@ -4,7 +4,7 @@ import subprocess
 from cffi import FFI
 
 mpc_dir = os.path.dirname(os.path.abspath(__file__))
-libmpc_fn = os.path.join(mpc_dir, "libcommampc.so")
+libmpc_fn = os.path.join(mpc_dir, "libmpc.so")
 subprocess.check_call(["make", "-j4"], cwd=mpc_dir)
 
 ffi = FFI()

selfdrive/controls/lib/longitudinal_mpc/Makefile

@@ -19,60 +19,52 @@ ACADO_LIBS := -L $(PHONELIBS)/acado/x64/lib -l:libacado_toolkit.a -l:libacado_ca
 endif
 
 OBJS = \
-  qp/Bounds.o \
+  lib_qp/Bounds.o \
-  qp/Constraints.o \
+  lib_qp/Constraints.o \
-  qp/CyclingManager.o \
+  lib_qp/CyclingManager.o \
-  qp/Indexlist.o \
+  lib_qp/Indexlist.o \
-  qp/MessageHandling.o \
+  lib_qp/MessageHandling.o \
-  qp/QProblem.o \
+  lib_qp/QProblem.o \
-  qp/QProblemB.o \
+  lib_qp/QProblemB.o \
-  qp/SubjectTo.o \
+  lib_qp/SubjectTo.o \
-  qp/Utils.o \
+  lib_qp/Utils.o \
-  qp/EXTRAS/SolutionAnalysis.o \
+  lib_qp/EXTRAS/SolutionAnalysis.o \
-	mpc_export/acado_qpoases_interface.o \
+	lib_mpc_export/acado_qpoases_interface.o \
-  mpc_export/acado_integrator.o \
+  lib_mpc_export/acado_integrator.o \
-  mpc_export/acado_solver.o \
+  lib_mpc_export/acado_solver.o \
-  mpc_export/acado_auxiliary_functions.o \
+  lib_mpc_export/acado_auxiliary_functions.o \
-  mpc.o
+  longitudinal_mpc.o
 
 DEPS := $(OBJS:.o=.d)
 
 .PHONY: all
-all: libcommampc1.so libcommampc2.so
+all: libmpc1.so libmpc2.so
 
-libcommampc1.so: $(OBJS)
+libmpc1.so: $(OBJS)
 	$(CXX) -shared -o '$@' $^ -lm
 
-libcommampc2.so: libcommampc1.so
+libmpc2.so: libmpc1.so
-	cp libcommampc1.so libcommampc2.so
+	cp libmpc1.so libmpc2.so
 
-qp/%.o: $(PHONELIBS)/qpoases/SRC/%.cpp
+lib_qp/%.o: $(PHONELIBS)/qpoases/SRC/%.cpp
 	@echo "[ CXX ] $@"
-	mkdir -p qp
+	mkdir -p lib_qp/EXTRAS
 	$(CXX) $(CXXFLAGS) -MMD \
-           -I mpc_export/ \
+           -I lib_mpc_export/ \
-           $(QPOASES_FLAGS) \
-           -c -o '$@' '$<'
-
-qp/EXTRAS/%.o: $(PHONELIBS)/qpoases/SRC/EXTRAS/%.cpp
-	@echo "[ CXX ] $@"
-	mkdir -p qp/EXTRAS
-	$(CXX) $(CXXFLAGS) -MMD \
-           -I mpc_export/ \
            $(QPOASES_FLAGS) \
            -c -o '$@' '$<'
 
 %.o: %.cpp
 	@echo "[ CXX ] $@"
 	$(CXX) $(CXXFLAGS) -MMD \
-           -I mpc_export/ \
+           -I lib_mpc_export/ \
            $(QPOASES_FLAGS) \
            -c -o '$@' '$<'
 
 %.o: %.c
 	@echo "[ CC ] $@"
 	$(CC) $(CFLAGS) -MMD \
-           -I mpc_export/ \
+           -I lib_mpc_export/ \
            $(QPOASES_FLAGS) \
            -c -o '$@' '$<'
 
@@ -89,6 +81,6 @@ generate: generator
 
 .PHONY: clean
 clean:
-	rm -f libcommampc1.so libcommampc2.so generator $(OBJS) $(DEPS)
+	rm -f *.so generator $(OBJS) $(DEPS)
 
 -include $(DEPS)

selfdrive/controls/lib/longitudinal_mpc/generator.cpp

@@ -18,15 +18,18 @@ int main( )
   DifferentialEquation f;
 
   DifferentialState x_ego, v_ego, a_ego;
-  DifferentialState x_l, v_l, a_l;
+  DifferentialState x_l, v_l, t;
 
-  OnlineData lambda;
+  OnlineData lambda, a_l_0;
 
   Control j_ego;
 
   auto desired = 4.0 + RW(v_ego, v_l);
   auto d_l = x_l - x_ego;
 
+  // Directly calculate a_l to prevent instabilites due to discretization
+  auto a_l = a_l_0 * exp(-lambda * t * t / 2);
+
   // Equations of motion
   f << dot(x_ego) == v_ego;
   f << dot(v_ego) == a_ego;
@@ -34,7 +37,7 @@ int main( )
 
   f << dot(x_l) == v_l;
   f << dot(v_l) == a_l;
-  f << dot(a_l) == -lambda * a_l;
+  f << dot(t) == 1;
 
   // Running cost
   Function h;
@@ -76,7 +79,7 @@ int main( )
   ocp.minimizeLSQEndTerm(QN, hN);
 
   ocp.subjectTo( 0.0 <= v_ego);
-  ocp.setNOD(1);
+  ocp.setNOD(2);
 
   OCPexport mpc(ocp);
   mpc.set( HESSIAN_APPROXIMATION, GAUSS_NEWTON );
@@ -94,7 +97,7 @@ int main( )
   mpc.set( GENERATE_MATLAB_INTERFACE, NO );
   mpc.set( GENERATE_SIMULINK_INTERFACE, NO );
 
-  if (mpc.exportCode( "mpc_export" ) != SUCCESSFUL_RETURN)
+  if (mpc.exportCode( "lib_mpc_export" ) != SUCCESSFUL_RETURN)
     exit( EXIT_FAILURE );
 
   mpc.printDimensionsQP( );

selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_auxiliary_functions.d

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

selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_auxiliary_functions.o

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

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

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

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

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

selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_integrator.d

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

selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_integrator.o

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

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

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

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

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

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

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

selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_solver.d

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

selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_solver.o

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

selfdrive/controls/lib/longitudinal_mpc/lib_qp/Bounds.d

@@ -0,0 +1,14 @@
+lib_qp/Bounds.o: ../../../../phonelibs/qpoases/SRC/Bounds.cpp \
+  ../../../../phonelibs/qpoases/INCLUDE/Bounds.hpp \
+  ../../../../phonelibs/qpoases/INCLUDE/SubjectTo.hpp \
+  ../../../../phonelibs/qpoases/INCLUDE/Indexlist.hpp \
+  ../../../../phonelibs/qpoases/INCLUDE/Utils.hpp \
+  ../../../../phonelibs/qpoases/INCLUDE/MessageHandling.hpp \
+  ../../../../phonelibs/qpoases/INCLUDE/Types.hpp \
+  ../../../../phonelibs/qpoases/INCLUDE/Constants.hpp \
+  lib_mpc_export/acado_qpoases_interface.hpp \
+  ../../../../phonelibs/qpoases/SRC/MessageHandling.ipp \
+  ../../../../phonelibs/qpoases/SRC/Utils.ipp \
+  ../../../../phonelibs/qpoases/SRC/Indexlist.ipp \
+  ../../../../phonelibs/qpoases/SRC/SubjectTo.ipp \
+  ../../../../phonelibs/qpoases/SRC/Bounds.ipp

selfdrive/controls/lib/longitudinal_mpc/lib_qp/Bounds.o

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

selfdrive/controls/lib/longitudinal_mpc/lib_qp/Constraints.d

@@ -0,0 +1,14 @@
+lib_qp/Constraints.o: ../../../../phonelibs/qpoases/SRC/Constraints.cpp \
+  ../../../../phonelibs/qpoases/INCLUDE/Constraints.hpp \
+  ../../../../phonelibs/qpoases/INCLUDE/SubjectTo.hpp \
+  ../../../../phonelibs/qpoases/INCLUDE/Indexlist.hpp \
+  ../../../../phonelibs/qpoases/INCLUDE/Utils.hpp \
+  ../../../../phonelibs/qpoases/INCLUDE/MessageHandling.hpp \
+  ../../../../phonelibs/qpoases/INCLUDE/Types.hpp \
+  ../../../../phonelibs/qpoases/INCLUDE/Constants.hpp \
+  lib_mpc_export/acado_qpoases_interface.hpp \
+  ../../../../phonelibs/qpoases/SRC/MessageHandling.ipp \
+  ../../../../phonelibs/qpoases/SRC/Utils.ipp \
+  ../../../../phonelibs/qpoases/SRC/Indexlist.ipp \
+  ../../../../phonelibs/qpoases/SRC/SubjectTo.ipp \
+  ../../../../phonelibs/qpoases/SRC/Constraints.ipp

selfdrive/controls/lib/longitudinal_mpc/lib_qp/Constraints.o

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

selfdrive/controls/lib/longitudinal_mpc/lib_qp/CyclingManager.d

@@ -0,0 +1,11 @@
+lib_qp/CyclingManager.o: \
+  ../../../../phonelibs/qpoases/SRC/CyclingManager.cpp \
+  ../../../../phonelibs/qpoases/INCLUDE/CyclingManager.hpp \
+  ../../../../phonelibs/qpoases/INCLUDE/Utils.hpp \
+  ../../../../phonelibs/qpoases/INCLUDE/MessageHandling.hpp \
+  ../../../../phonelibs/qpoases/INCLUDE/Types.hpp \
+  ../../../../phonelibs/qpoases/INCLUDE/Constants.hpp \
+  lib_mpc_export/acado_qpoases_interface.hpp \
+  ../../../../phonelibs/qpoases/SRC/MessageHandling.ipp \
+  ../../../../phonelibs/qpoases/SRC/Utils.ipp \
+  ../../../../phonelibs/qpoases/SRC/CyclingManager.ipp

selfdrive/controls/lib/longitudinal_mpc/lib_qp/CyclingManager.o

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

selfdrive/controls/lib/longitudinal_mpc/lib_qp/EXTRAS/SolutionAnalysis.d

@@ -0,0 +1,24 @@
+lib_qp/EXTRAS/SolutionAnalysis.o: \
+  ../../../../phonelibs/qpoases/SRC/EXTRAS/SolutionAnalysis.cpp \
+  ../../../../phonelibs/qpoases/INCLUDE/EXTRAS/SolutionAnalysis.hpp \
+  ../../../../phonelibs/qpoases/INCLUDE/QProblem.hpp \
+  ../../../../phonelibs/qpoases/INCLUDE/QProblemB.hpp \
+  ../../../../phonelibs/qpoases/INCLUDE/Bounds.hpp \
+  ../../../../phonelibs/qpoases/INCLUDE/SubjectTo.hpp \
+  ../../../../phonelibs/qpoases/INCLUDE/Indexlist.hpp \
+  ../../../../phonelibs/qpoases/INCLUDE/Utils.hpp \
+  ../../../../phonelibs/qpoases/INCLUDE/MessageHandling.hpp \
+  ../../../../phonelibs/qpoases/INCLUDE/Types.hpp \
+  ../../../../phonelibs/qpoases/INCLUDE/Constants.hpp \
+  lib_mpc_export/acado_qpoases_interface.hpp \
+  ../../../../phonelibs/qpoases/SRC/MessageHandling.ipp \
+  ../../../../phonelibs/qpoases/SRC/Utils.ipp \
+  ../../../../phonelibs/qpoases/SRC/Indexlist.ipp \
+  ../../../../phonelibs/qpoases/SRC/SubjectTo.ipp \
+  ../../../../phonelibs/qpoases/SRC/Bounds.ipp \
+  ../../../../phonelibs/qpoases/SRC/QProblemB.ipp \
+  ../../../../phonelibs/qpoases/INCLUDE/Constraints.hpp \
+  ../../../../phonelibs/qpoases/SRC/Constraints.ipp \
+  ../../../../phonelibs/qpoases/INCLUDE/CyclingManager.hpp \
+  ../../../../phonelibs/qpoases/SRC/CyclingManager.ipp \
+  ../../../../phonelibs/qpoases/SRC/QProblem.ipp

selfdrive/controls/lib/longitudinal_mpc/lib_qp/EXTRAS/SolutionAnalysis.o

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

selfdrive/controls/lib/longitudinal_mpc/lib_qp/Indexlist.d

@@ -0,0 +1,10 @@
+lib_qp/Indexlist.o: ../../../../phonelibs/qpoases/SRC/Indexlist.cpp \
+  ../../../../phonelibs/qpoases/INCLUDE/Indexlist.hpp \
+  ../../../../phonelibs/qpoases/INCLUDE/Utils.hpp \
+  ../../../../phonelibs/qpoases/INCLUDE/MessageHandling.hpp \
+  ../../../../phonelibs/qpoases/INCLUDE/Types.hpp \
+  ../../../../phonelibs/qpoases/INCLUDE/Constants.hpp \
+  lib_mpc_export/acado_qpoases_interface.hpp \
+  ../../../../phonelibs/qpoases/SRC/MessageHandling.ipp \
+  ../../../../phonelibs/qpoases/SRC/Utils.ipp \
+  ../../../../phonelibs/qpoases/SRC/Indexlist.ipp

selfdrive/controls/lib/longitudinal_mpc/lib_qp/Indexlist.o

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

selfdrive/controls/lib/longitudinal_mpc/lib_qp/MessageHandling.d

@@ -0,0 +1,9 @@
+lib_qp/MessageHandling.o: \
+  ../../../../phonelibs/qpoases/SRC/MessageHandling.cpp \
+  ../../../../phonelibs/qpoases/INCLUDE/MessageHandling.hpp \
+  ../../../../phonelibs/qpoases/INCLUDE/Types.hpp \
+  ../../../../phonelibs/qpoases/INCLUDE/Constants.hpp \
+  lib_mpc_export/acado_qpoases_interface.hpp \
+  ../../../../phonelibs/qpoases/SRC/MessageHandling.ipp \
+  ../../../../phonelibs/qpoases/INCLUDE/Utils.hpp \
+  ../../../../phonelibs/qpoases/SRC/Utils.ipp

selfdrive/controls/lib/longitudinal_mpc/lib_qp/MessageHandling.o

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

selfdrive/controls/lib/longitudinal_mpc/lib_qp/QProblem.d

@@ -0,0 +1,22 @@
+lib_qp/QProblem.o: ../../../../phonelibs/qpoases/SRC/QProblem.cpp \
+  ../../../../phonelibs/qpoases/INCLUDE/QProblem.hpp \
+  ../../../../phonelibs/qpoases/INCLUDE/QProblemB.hpp \
+  ../../../../phonelibs/qpoases/INCLUDE/Bounds.hpp \
+  ../../../../phonelibs/qpoases/INCLUDE/SubjectTo.hpp \
+  ../../../../phonelibs/qpoases/INCLUDE/Indexlist.hpp \
+  ../../../../phonelibs/qpoases/INCLUDE/Utils.hpp \
+  ../../../../phonelibs/qpoases/INCLUDE/MessageHandling.hpp \
+  ../../../../phonelibs/qpoases/INCLUDE/Types.hpp \
+  ../../../../phonelibs/qpoases/INCLUDE/Constants.hpp \
+  lib_mpc_export/acado_qpoases_interface.hpp \
+  ../../../../phonelibs/qpoases/SRC/MessageHandling.ipp \
+  ../../../../phonelibs/qpoases/SRC/Utils.ipp \
+  ../../../../phonelibs/qpoases/SRC/Indexlist.ipp \
+  ../../../../phonelibs/qpoases/SRC/SubjectTo.ipp \
+  ../../../../phonelibs/qpoases/SRC/Bounds.ipp \
+  ../../../../phonelibs/qpoases/SRC/QProblemB.ipp \
+  ../../../../phonelibs/qpoases/INCLUDE/Constraints.hpp \
+  ../../../../phonelibs/qpoases/SRC/Constraints.ipp \
+  ../../../../phonelibs/qpoases/INCLUDE/CyclingManager.hpp \
+  ../../../../phonelibs/qpoases/SRC/CyclingManager.ipp \
+  ../../../../phonelibs/qpoases/SRC/QProblem.ipp

selfdrive/controls/lib/longitudinal_mpc/lib_qp/QProblem.o

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

selfdrive/controls/lib/longitudinal_mpc/lib_qp/QProblemB.d

@@ -0,0 +1,16 @@
+lib_qp/QProblemB.o: ../../../../phonelibs/qpoases/SRC/QProblemB.cpp \
+  ../../../../phonelibs/qpoases/INCLUDE/QProblemB.hpp \
+  ../../../../phonelibs/qpoases/INCLUDE/Bounds.hpp \
+  ../../../../phonelibs/qpoases/INCLUDE/SubjectTo.hpp \
+  ../../../../phonelibs/qpoases/INCLUDE/Indexlist.hpp \
+  ../../../../phonelibs/qpoases/INCLUDE/Utils.hpp \
+  ../../../../phonelibs/qpoases/INCLUDE/MessageHandling.hpp \
+  ../../../../phonelibs/qpoases/INCLUDE/Types.hpp \
+  ../../../../phonelibs/qpoases/INCLUDE/Constants.hpp \
+  lib_mpc_export/acado_qpoases_interface.hpp \
+  ../../../../phonelibs/qpoases/SRC/MessageHandling.ipp \
+  ../../../../phonelibs/qpoases/SRC/Utils.ipp \
+  ../../../../phonelibs/qpoases/SRC/Indexlist.ipp \
+  ../../../../phonelibs/qpoases/SRC/SubjectTo.ipp \
+  ../../../../phonelibs/qpoases/SRC/Bounds.ipp \
+  ../../../../phonelibs/qpoases/SRC/QProblemB.ipp