diff --git a/Dockerfile.openpilot b/Dockerfile.openpilot
index 98e1c0abf5555d..2428249fae61ee 100644
--- a/Dockerfile.openpilot
+++ b/Dockerfile.openpilot
@@ -2,7 +2,7 @@ FROM ubuntu:16.04
 ENV PYTHONUNBUFFERED 1
 
 RUN apt-get update && apt-get install -y build-essential clang vim screen wget bzip2 git libglib2.0-0 python-pip capnproto libcapnp-dev libzmq5-dev libffi-dev libusb-1.0-0
-RUN pip install numpy==1.11.2 scipy==0.18.1 matplotlib
+RUN pip install numpy==1.11.2 scipy==0.18.1 matplotlib==2.1.2
 
 COPY requirements_openpilot.txt /tmp/
 RUN pip install -r /tmp/requirements_openpilot.txt
@@ -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
diff --git a/RELEASES.md b/RELEASES.md
index 0cc746f3238bf6..6230b33fe05f9f 100644
--- a/RELEASES.md
+++ b/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!
diff --git a/cereal/car.capnp b/cereal/car.capnp
index ff1297f1ef8095..6b2e95f6cf3e15 100644
--- a/cereal/car.capnp
+++ b/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;
   }
 }
 
diff --git a/cereal/log.capnp b/cereal/log.capnp
index 4f4e224f2f9fcb..ea7012bad397b2 100644
--- a/cereal/log.capnp
+++ b/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
diff --git a/common/dbc.py b/common/dbc.py
index 2a5e4f86883361..6accad43f82bec 100755
--- a/common/dbc.py
+++ b/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 @@ def int_or_float(s):
   "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 @@ def lookup_msg_id(self, msg_id):
       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 @@ def encode(self, msg_id, dd):
     """
     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:
-          tbs = bitstring.Bits(int=ival, length=s.size)
-        else:
-          tbs = bitstring.Bits(uint=ival, length=s.size)
+        shift = b1 if s.is_little_endian else bo
+        mask = ((1 << b2) - 1) << shift
+        dat = (ival & ((1 << b2) - 1)) << shift
+
+        if s.is_little_endian:
+          mask = self.reverse_bytes(mask)
+          dat = self.reverse_bytes(dat)
 
-        lpad = bitstring.Bits(bin="0b"+"0"*ss)
-        rpad = bitstring.Bits(bin="0b"+"0"*(8*size-(ss+s.size)))
-        tbs = lpad+tbs+rpad
+        result &= ~mask
+        result |= dat
 
-        bsf |= tbs
-    return bsf.tobytes()
+    result = struct.pack('>Q', result)
+    return result[:size]
 
   def decode(self, x, arr=None, debug=False):
     """Decode a CAN message using the dbc.
@@ -195,55 +210,77 @@ def decode(self, x, arr=None, debug=False):
     if debug:
       print name
 
-    blen = 8*len(x[2])
-
-    st = x[2].rjust(8, '\x00')
+    st = x[2].ljust(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?
-      #   see http://vi-firmware.openxcplatform.com/en/master/config/bit-numbering.html
-      if s[3] is False:
-        ss = self.bits_index[s[1]]
-        if be is None:
-          be = struct.unpack(">Q", st)[0]
-        x2_int = be
-        data_bit_pos = (blen - (ss + s[2]))
+      start_bit = s[1]
+      signal_size = s[2]
+      little_endian = s[3]
+      signed = s[4]
+      factor = s[5]
+      offset = s[6]
+
+      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)
-        ss = s[1]
-        data_bit_pos = ss
+        shift_amount = b1
+        tmp = le
+      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
-        ival -= (1<<s[2])
+      tmp = (tmp >> shift_amount) & ((1 << b2) - 1)
+      if signed and (tmp >> (b2 - 1)):
+        tmp -= (1 << b2)
 
-      # 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])
+      tmp = tmp * factor + offset
+
+      # if debug:
+      #   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]))
-   print dbc_test.get_signals(0xe4)
+   assert decoded == msg
diff --git a/common/filter_simple.py b/common/filter_simple.py
new file mode 100644
index 00000000000000..a3206db1cc5684
--- /dev/null
+++ b/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
+
+
diff --git a/common/transformations/model.py b/common/transformations/model.py
index 1a8122d6e34c35..26a20d509fd970 100644
--- a/common/transformations/model.py
+++ b/common/transformations/model.py
@@ -57,6 +57,8 @@
 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([
diff --git a/opendbc/cadillac_ct6_powertrain.dbc b/opendbc/cadillac_ct6_powertrain.dbc
index 8fdd4c17e1a715..f139b1dbd4a734 100644
--- a/opendbc/cadillac_ct6_powertrain.dbc
+++ b/opendbc/cadillac_ct6_powertrain.dbc
@@ -189,7 +189,7 @@ BO_ 1033 ASCMKeepAlive: 7 NEO
  SG_ ASCMKeepAliveAllZero : 7|56@0+ (1,0) [0|0] ""  NEO
 
 BO_ 1217 ECMEngineCoolantTemp: 8 K20_ECM
- SG_ EngineCoolantTemp : 23|8@0+ (1,-40) [0|0] "°C"  NEO
+ SG_ EngineCoolantTemp : 23|8@0+ (1,-40) [0|0] "°C"  NEO
 
 BO_ 1249 VIN_Part2: 8 K20_ECM
  SG_ VINPart2 : 7|64@0+ (1,0) [0|0] ""  NEO
diff --git a/opendbc/generator/honda/honda_civic_touring_2016_can.dbc b/opendbc/generator/honda/honda_civic_touring_2016_can.dbc
index 9e795fbae38c6d..9974d7068a3bfb 100644
--- a/opendbc/generator/honda/honda_civic_touring_2016_can.dbc
+++ b/opendbc/generator/honda/honda_civic_touring_2016_can.dbc
@@ -126,7 +126,7 @@ BO_ 1302 ODOMETER: 8 XXX
 
 CM_ SG_ 401 GEAR "10 = reverse, 11 = transition";
 CM_ SG_ 420 BRAKE_HOLD_RELATED "On when Brake Hold engaged";
-CM_ SG_ 450 EPB_STATE "3 "engaged" 2 "disengaging" 1 "engaging" 0 "disengaged"";
+CM_ SG_ 450 EPB_STATE "3 \"engaged\" 2 \"disengaging\" 1 \"engaging\" 0 \"disengaged\"";
 CM_ SG_ 806 REVERSE_LIGHT "Might be reverse gear selected and not the lights";
 
 VAL_ 399 STEER_STATUS 5 "fault" 4 "no_torque_alert_2" 2 "no_torque_alert_1" 0 "normal" ;
diff --git a/opendbc/gm_global_a_powertrain.dbc b/opendbc/gm_global_a_powertrain.dbc
index 93d1291aa6c856..70105490826601 100644
--- a/opendbc/gm_global_a_powertrain.dbc
+++ b/opendbc/gm_global_a_powertrain.dbc
@@ -180,7 +180,7 @@ BO_ 1033 ASCMKeepAlive: 7 NEO
 BO_ 1034 ASCM_40A: 7 K124_ASCM
 
 BO_ 1217 ECMEngineCoolantTemp: 8 K20_ECM
- SG_ EngineCoolantTemp : 23|8@0+ (1,-40) [0|0] "°C"  NEO
+ SG_ EngineCoolantTemp : 23|8@0+ (1,-40) [0|0] "°C"  NEO
 
 BO_ 1249 VIN_Part2: 8 K20_ECM
  SG_ VINPart2 : 7|64@0+ (1,0) [0|0] ""  NEO
diff --git a/opendbc/honda_civic_touring_2016_can_generated.dbc b/opendbc/honda_civic_touring_2016_can_generated.dbc
index 0ec2f5846c69a2..ff5f80caad9831 100644
--- a/opendbc/honda_civic_touring_2016_can_generated.dbc
+++ b/opendbc/honda_civic_touring_2016_can_generated.dbc
@@ -355,7 +355,7 @@ BO_ 1302 ODOMETER: 8 XXX
 
 CM_ SG_ 401 GEAR "10 = reverse, 11 = transition";
 CM_ SG_ 420 BRAKE_HOLD_RELATED "On when Brake Hold engaged";
-CM_ SG_ 450 EPB_STATE "3 "engaged" 2 "disengaging" 1 "engaging" 0 "disengaged"";
+CM_ SG_ 450 EPB_STATE "3 \"engaged\" 2 \"disengaging\" 1 \"engaging\" 0 \"disengaged\"";
 CM_ SG_ 806 REVERSE_LIGHT "Might be reverse gear selected and not the lights";
 
 VAL_ 399 STEER_STATUS 5 "fault" 4 "no_torque_alert_2" 2 "no_torque_alert_1" 0 "normal" ;
diff --git a/opendbc/honda_odyssey_extreme_edition_2018_china_can.dbc b/opendbc/honda_odyssey_extreme_edition_2018_china_can.dbc
new file mode 100644
index 00000000000000..ad66716f35cc01
--- /dev/null
+++ b/opendbc/honda_odyssey_extreme_edition_2018_china_can.dbc
@@ -0,0 +1,295 @@
+VERSION ""
+
+
+NS_ :
+    NS_DESC_
+    CM_
+    BA_DEF_
+    BA_
+    VAL_
+    CAT_DEF_
+    CAT_
+    FILTER
+    BA_DEF_DEF_
+    EV_DATA_
+    ENVVAR_DATA_
+    SGTYPE_
+    SGTYPE_VAL_
+    BA_DEF_SGTYPE_
+    BA_SGTYPE_
+    SIG_TYPE_REF_
+    VAL_TABLE_
+    SIG_GROUP_
+    SIG_VALTYPE_
+    SIGTYPE_VALTYPE_
+    BO_TX_BU_
+    BA_DEF_REL_
+    BA_REL_
+    BA_DEF_DEF_REL_
+    BU_SG_REL_
+    BU_EV_REL_
+    BU_BO_REL_
+    SG_MUL_VAL_
+
+BS_:
+
+BU_: EBCM ADAS PCM EPS VSA SCM BDY XXX EPB EON INTERCEPTOR
+
+
+BO_ 344 ENGINE_DATA: 8 PCM
+ SG_ XMISSION_SPEED : 7|16@0+ (0.01,0) [0|250] "kph" EON
+ SG_ XMISSION_SPEED2 : 39|16@0+ (0.01,0) [0|250] "kph" EON
+ SG_ ODOMETER : 55|8@0+ (10,0) [0|2550] "m" XXX
+ SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" EON
+ SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" EON
+ SG_ ZEROS : 23|16@0+ (1,0) [0|15000] "" EON
+
+BO_ 380 POWERTRAIN_DATA: 8 PCM
+ SG_ PEDAL_GAS : 7|8@0+ (1,0) [0|255] "" EON
+ SG_ ENGINE_RPM : 23|16@0+ (1,0) [0|15000] "rpm" EON
+ SG_ GAS_PRESSED : 39|1@0+ (1,0) [0|1] "" EON
+ SG_ ACC_STATUS : 38|1@0+ (1,0) [0|1] "rpm" EON
+ SG_ BOH_17C : 37|5@0+ (1,0) [0|1] "rpm" EON
+ SG_ BRAKE_SWITCH : 32|1@0+ (1,0) [0|1] "rpm" EON
+ SG_ BOH2_17C : 47|10@0+ (1,0) [0|1] "rpm" EON
+ SG_ BRAKE_PRESSED : 53|1@0+ (1,0) [0|1] "" EON
+ SG_ BOH3_17C : 52|5@0+ (1,0) [0|1] "rpm" EON
+ SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" EON
+ SG_ CHECKSUM : 59|4@0+ (1,0) [0|3] "" EON
+
+BO_ 432 STANDSTILL: 7 VSA
+ SG_ CONTROLLED_STANDSTILL : 0|1@0+ (1,0) [0|1] "" EON
+ SG_ WHEELS_MOVING : 12|1@0+ (1,0) [0|1] "" EON
+ SG_ BRAKE_ERROR_1 : 11|1@0+ (1,0) [0|1] "" EON
+ SG_ BRAKE_ERROR_2 : 9|1@0+ (1,0) [0|1] "" EON
+ SG_ COUNTER : 53|2@0+ (1,0) [0|3] "" EON
+ SG_ CHECKSUM : 51|4@0+ (1,0) [0|3] "" EON
+
+BO_ 464 WHEEL_SPEEDS: 8 VSA
+ SG_ WHEEL_SPEED_FL : 7|15@0+ (0.01,0) [0|250] "kph" EON
+ SG_ WHEEL_SPEED_FR : 8|15@0+ (0.01,0) [0|250] "kph" EON
+ SG_ WHEEL_SPEED_RL : 25|15@0+ (0.01,0) [0|250] "kph" EON
+ SG_ WHEEL_SPEED_RR : 42|15@0+ (0.01,0) [0|250] "kph" EON
+ SG_ CHECKSUM : 59|4@0+ (1,0) [0|3] "" EON
+
+BO_ 490 VEHICLE_DYNAMICS: 8 VSA
+ SG_ LONG_ACCEL : 23|16@0- (0.0015384,0) [-20|20] "m/s2" EON
+ SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" EON
+ SG_ CHECKSUM : 59|4@0+ (1,0) [0|3] "" EON
+
+BO_ 506 BRAKE_COMMAND: 8 ADAS
+ SG_ COMPUTER_BRAKE : 7|10@0+ (1,0) [0|1] "" EBCM
+ SG_ ZEROS_BOH : 13|5@0+ (1,0) [0|1] "" EBCM
+ SG_ BRAKE_PUMP_REQUEST : 8|1@0+ (1,0) [0|1] "" EBCM
+ SG_ CRUISE_BOH2 : 23|3@0+ (1,0) [0|1] "" EBCM
+ SG_ CRUISE_OVERRIDE : 20|1@0+ (1,0) [0|1] "" EBCM
+ SG_ CRUISE_BOH3 : 19|1@0+ (1,0) [0|1] "" EBCM
+ SG_ CRUISE_FAULT_CMD : 18|1@0+ (1,0) [0|1] "" EBCM
+ SG_ CRUISE_CANCEL_CMD : 17|1@0+ (1,0) [0|1] "" EBCM
+ SG_ COMPUTER_BRAKE_REQUEST : 16|1@0+ (1,0) [0|1] "" EBCM
+ SG_ SET_ME_0X80 : 31|8@0+ (1,0) [0|1] "" EBCM
+ SG_ BRAKE_LIGHTS : 39|1@0+ (1,0) [0|1] "" EBCM
+ SG_ CRUISE_STATES : 38|7@0+ (1,0) [0|1] "" EBCM
+ SG_ CHIME : 47|3@0+ (1,0) [0|7] "" EBCM
+ SG_ ZEROS_BOH6 : 44|1@0+ (1,0) [0|1] "" EBCM
+ SG_ FCW : 43|2@0+ (1,0) [0|3] "" EBCM
+ SG_ ZEROS_BOH4 : 55|8@0+ (1,0) [0|0] "" EBCM
+ SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" EBCM
+ SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" EBCM
+
+BO_ 512 GAS_COMMAND: 6 EON
+ SG_ GAS_COMMAND : 7|16@0+ (0.253984064,-83.3) [0|1] "" INTERCEPTOR
+ SG_ GAS_COMMAND2 : 23|16@0+ (0.126992032,-83.3) [0|1] "" INTERCEPTOR
+ SG_ ENABLE : 39|1@0+ (1,0) [0|1] "" INTERCEPTOR
+ SG_ COUNTER : 45|2@0+ (1,0) [0|3] "" INTERCEPTOR
+ SG_ CHECKSUM : 43|4@0+ (1,0) [0|3] "" INTERCEPTOR
+
+BO_ 513 GAS_SENSOR: 6 INTERCEPTOR
+ SG_ INTERCEPTOR_GAS : 7|16@0+ (0.253984064,-83.3) [0|1] "" EON
+ SG_ INTERCEPTOR_GAS2 : 23|16@0+ (0.126992032,-83.3) [0|1] "" EON
+ SG_ STATE : 39|8@0+ (1,0) [0|255] "" EON
+ SG_ COUNTER : 45|2@0+ (1,0) [0|3] "" EON
+ SG_ CHECKSUM : 43|4@0+ (1,0) [0|3] "" EON
+
+BO_ 597 ROUGH_WHEEL_SPEED: 8 VSA
+ SG_ WHEEL_SPEED_FL : 7|8@0+ (1,0) [0|255] "mph" EON
+ SG_ WHEEL_SPEED_FR : 15|8@0+ (1,0) [0|255] "mph" EON
+ SG_ WHEEL_SPEED_RL : 23|8@0+ (1,0) [0|255] "mph" EON
+ SG_ WHEEL_SPEED_RR : 31|8@0+ (1,0) [0|255] "mph" EON
+ SG_ SET_TO_X55 : 39|8@0+ (1,0) [0|255] "" EON
+ SG_ SET_TO_X55_2 : 47|8@0+ (1,0) [0|255] "" EON
+ SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" EON
+ SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" EON
+
+BO_ 773 SEATBELT_STATUS: 7 BDY
+ SG_ SEATBELT_DRIVER_LAMP : 7|1@0+ (1,0) [0|1] "" EON
+ SG_ SEATBELT_PASS_UNLATCHED : 10|1@0+ (1,0) [0|1] "" EON
+ SG_ SEATBELT_PASS_LATCHED : 11|1@0+ (1,0) [0|1] "" EON
+ SG_ SEATBELT_DRIVER_UNLATCHED : 12|1@0+ (1,0) [0|1] "" EON
+ SG_ SEATBELT_DRIVER_LATCHED : 13|1@0+ (1,0) [0|1] "" EON
+ SG_ PASS_AIRBAG_OFF : 14|1@0+ (1,0) [0|1] "" EON
+ SG_ PASS_AIRBAG_ON : 15|1@0+ (1,0) [0|1] "" EON
+ SG_ COUNTER : 53|2@0+ (1,0) [0|3] "" EON
+ SG_ CHECKSUM : 51|4@0+ (1,0) [0|3] "" EON
+
+BO_ 777 LOCK_STATUS: 8 XXX
+ SG_ DOORS_UNLOCKED : 54|1@0+ (1,0) [0|1] "" EON
+ SG_ DOORS_LOCKED : 55|1@0+ (1,0) [0|1] "" EON
+ SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" EON
+ SG_ CHECKSUM : 59|4@0+ (1,0) [0|3] "" EON
+
+BO_ 780 ACC_HUD: 8 ADAS
+ SG_ PCM_SPEED : 7|16@0+ (0.01,0) [0|250] "kph" BDY
+ SG_ PCM_GAS : 23|8@0+ (1,0) [0|127] "" BDY
+ SG_ CRUISE_SPEED : 31|8@0+ (1,0) [0|255] "" BDY
+ SG_ DTC_MODE : 39|1@0+ (1,0) [0|1] "" BDY
+ SG_ BOH : 38|1@0+ (1,0) [0|1] "" BDY
+ SG_ ACC_PROBLEM : 37|1@0+ (1,0) [0|1] "" BDY
+ SG_ FCM_OFF : 36|1@0+ (1,0) [0|1] "" BDY
+ SG_ BOH_2 : 35|1@0+ (1,0) [0|1] "" BDY
+ SG_ FCM_PROBLEM : 34|1@0+ (1,0) [0|1] "" BDY
+ SG_ RADAR_OBSTRUCTED : 33|1@0+ (1,0) [0|1] "" BDY
+ SG_ ENABLE_MINI_CAR : 32|1@0+ (1,0) [0|1] "" BDY
+ SG_ SET_ME_X03 : 47|2@0+ (1,0) [0|3] "" BDY
+ SG_ HUD_LEAD : 45|2@0+ (1,0) [0|3] "" BDY
+ SG_ BOH_3 : 43|1@0+ (1,0) [0|3] "" BDY
+ SG_ BOH_4 : 42|1@0+ (1,0) [0|3] "" BDY
+ SG_ BOH_5 : 41|1@0+ (1,0) [0|3] "" BDY
+ SG_ CRUISE_CONTROL_LABEL : 40|1@0+ (1,0) [0|3] "" BDY
+ SG_ HUD_DISTANCE_3 : 52|1@0+ (1,0) [0|1] "" BDY
+ SG_ SET_ME_X03_2 : 55|2@0+ (1,0) [0|3] "" BDY
+ SG_ SET_ME_X01 : 48|1@0+ (1,0) [0|1] "" BDY
+ SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" BDY
+ SG_ CHECKSUM : 59|4@0+ (1,0) [0|3] "" BDY
+
+BO_ 804 CRUISE: 8 PCM
+ SG_ HUD_SPEED_KPH : 7|8@0+ (1,0) [0|255] "kph" EON
+ SG_ HUD_SPEED_MPH : 15|8@0+ (1,0) [0|255] "mph" EON
+ SG_ TRIP_FUEL_CONSUMED : 23|16@0+ (1,0) [0|255] "" EON
+ SG_ CRUISE_SPEED_PCM : 39|8@0+ (1,0) [0|255] "" EON
+ SG_ BOH2 : 47|8@0- (1,0) [0|255] "" EON
+ SG_ BOH3 : 55|8@0+ (1,0) [0|255] "" EON
+ SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" EON
+ SG_ CHECKSUM : 59|4@0+ (1,0) [0|3] "" EON
+
+BO_ 829 LKAS_HUD: 5 ADAS
+ SG_ CAM_TEMP_HIGH : 7|1@0+ (1,0) [0|255] "" BDY
+ SG_ SET_ME_X41 : 6|7@0+ (1,0) [0|127] "" BDY
+ SG_ BOH : 6|7@0+ (1,0) [0|127] "" BDY
+ SG_ DASHED_LANES : 14|1@0+ (1,0) [0|1] "" BDY
+ SG_ DTC : 13|1@0+ (1,0) [0|1] "" BDY
+ SG_ LKAS_PROBLEM : 12|1@0+ (1,0) [0|1] "" BDY
+ SG_ LKAS_OFF : 11|1@0+ (1,0) [0|1] "" BDY
+ SG_ SOLID_LANES : 10|1@0+ (1,0) [0|1] "" BDY
+ SG_ LDW_RIGHT : 9|1@0+ (1,0) [0|1] "" BDY
+ SG_ STEERING_REQUIRED : 8|1@0+ (1,0) [0|1] "" BDY
+ SG_ BOH2 : 23|2@0+ (1,0) [0|4] "" BDY
+ SG_ LDW_PROBLEM : 21|1@0+ (1,0) [0|1] "" BDY
+ SG_ BEEP : 17|2@0+ (1,0) [0|1] "" BDY
+ SG_ LDW_ON : 28|1@0+ (1,0) [0|1] "" BDY
+ SG_ LDW_OFF : 27|1@0+ (1,0) [0|1] "" BDY
+ SG_ CLEAN_WINDSHIELD : 26|1@0+ (1,0) [0|1] "" BDY
+ SG_ SET_ME_X48 : 31|8@0+ (1,0) [0|255] "" BDY
+ SG_ COUNTER : 37|2@0+ (1,0) [0|3] "" BDY
+ SG_ CHECKSUM : 35|4@0+ (1,0) [0|15] "" BDY
+
+BO_ 892 CRUISE_PARAMS: 8 PCM
+ SG_ CRUISE_SPEED_OFFSET : 31|8@0- (0.1,0) [-128|127] "kph" EON
+ SG_ CHECKSUM : 59|4@0+ (1,0) [0|3] "" EON
+ SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" EON
+
+BO_ 316 GAS_PEDAL: 8 PCM
+ SG_ CAR_GAS : 39|8@0+ (1,0) [0|255] "" EON
+
+BO_ 342 STEERING_SENSORS: 6 EPS
+ SG_ STEER_ANGLE_RATE : 23|16@0- (1,0) [-3000|3000] "deg/s" EON
+ SG_ COUNTER : 45|2@0+ (1,0) [0|3] "" EON
+ SG_ CHECKSUM : 43|4@0+ (1,0) [0|15] "" EON
+ SG_ STEER_ANGLE : 7|16@0- (-0.1,0) [-500|500] "deg" EON
+
+BO_ 399 STEER_STATUS: 7 EPS
+ SG_ STEER_TORQUE_SENSOR : 7|16@0- (1,0) [-2985|2985] "tbd" EON
+ SG_ STEER_TORQUE_MOTOR : 23|16@0- (1,0) [-31000|31000] "tbd" EON
+ SG_ CHECKSUM : 51|4@0+ (1,0) [0|15] "" EON
+ SG_ COUNTER : 53|2@0+ (1,0) [0|3] "" EON
+ SG_ STEER_STATUS : 43|4@0+ (1,0) [0|15] "" EON
+ SG_ STEER_CONTROL_ACTIVE : 35|1@0+ (1,0) [0|1] "" EON
+
+BO_ 401 GEARBOX: 8 PCM
+ SG_ GEAR_SHIFTER : 5|6@0+ (1,0) [0|63] "" EON
+ SG_ GEAR : 35|4@0+ (1,0) [0|15] "" EON
+ SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" EON
+ SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" EON
+
+BO_ 404 STEERING_CONTROL: 4 EON
+ SG_ SET_ME_X00 : 22|7@0+ (1,0) [0|127] "" EPS
+ SG_ STEER_TORQUE_REQUEST : 23|1@0+ (1,0) [0|1] "" EPS
+ SG_ COUNTER : 29|2@0+ (1,0) [0|15] "" EPS
+ SG_ CHECKSUM : 27|4@0+ (1,0) [0|3] "" EPS
+ SG_ STEER_TORQUE : 7|16@0- (-1,0) [-32767|32767] "" EPS
+
+BO_ 420 VSA_STATUS: 8 VSA
+ SG_ USER_BRAKE : 7|16@0+ (0.015625,-1.609375) [0|1000] "" EON
+ SG_ ESP_DISABLED : 28|1@0+ (1,0) [0|1] "" EON
+ SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" EON
+ SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" EON
+
+BO_ 422 SCM_BUTTONS: 8 SCM
+ SG_ CRUISE_BUTTONS : 7|3@0+ (1,0) [0|7] "" EON
+ SG_ LIGHTS_SETTING : 1|2@0+ (1,0) [0|3] "" EON
+ SG_ MAIN_ON : 47|1@0+ (1,0) [0|1] "" EON
+ SG_ CRUISE_SETTING : 43|2@0+ (1,0) [0|3] "" EON
+ SG_ DRIVERS_DOOR_OPEN : 63|1@0+ (1,0) [0|1] "" XXX
+ SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" EON
+ SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" EON
+
+BO_ 450 EPB_STATUS: 8 XXX
+ SG_ EPB_BRAKE_AND_PULL : 6|1@0+ (1,0) [0|1] "" XXX
+ SG_ EPB_ACTIVE : 3|1@0+ (1,0) [0|1] "" XXX
+ SG_ EPB_STATE : 29|2@0+ (1,0) [0|1] "" XXX
+ SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" XXX
+ SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" XXX
+
+BO_ 660 SCM_FEEDBACK: 8 SCM
+ SG_ RIGHT_BLINKER : 6|1@0+ (1,0) [0|1] "" EON
+ SG_ LEFT_BLINKER : 5|1@0+ (1,0) [0|1] "" EON
+ SG_ WIPERS_SPEED : 4|2@0+ (1,0) [0|3] "" EON
+ SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" EON
+ SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" EON
+
+BO_ 862 HIGHBEAM_CONTROL: 8 ADAS
+ SG_ ZEROS_BOH : 7|50@0+ (1,0) [0|127] "" BDY
+ SG_ ZEROS_BOH_2 : 48|4@1+ (1,0) [0|15] "" XXX
+ SG_ HIGHBEAMS_ON : 52|1@0+ (1,0) [0|1] "" XXX
+ SG_ AUTO_HIGHBEAMS_ACTIVE : 53|1@0+ (1,0) [0|1] "" XXX
+ SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" XXX
+ SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" XXX
+
+BO_ 1302 ODOMETER: 8 XXX
+ SG_ ODOMETER : 7|24@0+ (1,0) [0|16777215] "km" EON
+ SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" EON
+ SG_ CHECKSUM : 59|4@0+ (1,0) [0|3] "" EON
+
+
+
+
+CM_ SG_ 490 LONG_ACCEL "wheel speed derivative, noisy and zero snapping";
+CM_ SG_ 773 PASS_AIRBAG_OFF "Might just be indicator light";
+CM_ SG_ 773 PASS_AIRBAG_ON "Might just be indicator light";
+CM_ SG_ 780 CRUISE_SPEED "255 = no speed";
+CM_ SG_ 804 CRUISE_SPEED_PCM "255 = no speed";
+CM_ SG_ 829 BEEP "beeps are pleasant, chimes are for warnngs etc...";
+CM_ SG_ 401 GEAR "10 = reverse, 11 = transition";
+VAL_ 506 CHIME 4 "double_chime" 3 "single_chime" 2 "continuous_chime" 1 "repeating_chime" 0 "no_chime" ;
+VAL_ 506 FCW 3 "fcw" 2 "fcw" 1 "fcw" 0 "no_fcw" ;
+VAL_ 513 STATE 5 "FAULT_TIMEOUT" 4 "FAULT_STARTUP" 3 "FAULT_SCE" 2 "FAULT_SEND" 1 "FAULT_BAD_CHECKSUM" 0 "NO_FAULT" ;
+VAL_ 780 CRUISE_SPEED 255 "no_speed" 252 "stopped" ;
+VAL_ 780 HUD_LEAD 3 "acc_off" 2 "solid_car" 1 "dashed_car" 0 "no_car" ;
+VAL_ 829 BEEP 3 "single_beep" 2 "triple_beep" 1 "repeated_beep" 0 "no_beep" ;
+VAL_ 399 STEER_STATUS 5 "fault" 4 "no_torque_alert_2" 2 "no_torque_alert_1" 0 "normal" ;
+VAL_ 401 GEAR_SHIFTER 32 "L" 16 "S" 8 "D" 4 "N" 2 "R" 1 "P" ;
+VAL_ 401 GEAR 7 "L" 10 "S" 4 "D" 3 "N" 2 "R" 1 "P" ;
+VAL_ 422 CRUISE_BUTTONS 7 "tbd" 6 "tbd" 5 "tbd" 4 "accel_res" 3 "decel_set" 2 "cancel" 1 "main" 0 "none" ;
+VAL_ 422 LIGHTS_SETTING 3 "high_beam" 2 "low_beam" 1 "position" 0 "no_lights" ;
+VAL_ 422 CRUISE_SETTING 3 "distance_adj" 2 "tbd" 1 "lkas_button" 0 "none" ;
+CM_ "CHFFR_METRIC 342 STEER_ANGLE STEER_ANGLE 0.36 180; CHFFR_METRIC 380 ENGINE_RPM ENGINE_RPM 1 0; CHFFR_METRIC 804 ENGINE_TEMPERATURE ENGINE_TEMPERATURE 1 0";
diff --git a/opendbc/mercedes_benz_e350_2010.dbc b/opendbc/mercedes_benz_e350_2010.dbc
index 9841af6d4f8952..da6ae4c15ee66b 100644
--- a/opendbc/mercedes_benz_e350_2010.dbc
+++ b/opendbc/mercedes_benz_e350_2010.dbc
@@ -36,27 +36,141 @@ BS_:
 BU_: XXX
 
 
-BO_ 3 NEW_MSG_1: 8 XXX
- SG_ STEERING_ANGLE : 7|32@0- (1,0) [0|4294967295] "" XXX
- SG_ NEW_SIGNAL_1 : 55|8@0+ (1,0) [0|255] "" XXX
+BO_ 3 STEER_SENSOR: 8 XXX
+ SG_ COUNTER : 55|4@0+ (1,0) [0|15] "" XXX
+ SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX
+ SG_ STEER_RATE : 19|12@0- (0.5,0) [0|255] "" XXX
+ SG_ STEER_DIRECTION : 4|1@0+ (1,2) [0|1] "" XXX
+ SG_ STEER_ANGLE : 3|12@0- (-0.5,0) [-500|500] "degrees" XXX
 
-BO_ 5 NEW_MSG_2: 8 XXX
- SG_ BRAKE_POSITION : 23|16@0+ (1,0) [0|65535] "" XXX
- SG_ BRAKE_PRESSED : 0|8@1+ (1,0) [0|17] "" XXX
+BO_ 5 BRAKE_MODULE: 8 XXX
+ SG_ COUNTER : 55|4@0+ (1,0) [0|15] "" XXX
+ SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX
+ SG_ BRAKE_HOLD : 2|1@0+ (1,0) [0|1] "" XXX
+ SG_ BRAKE_POSITION : 17|10@0+ (1,0) [0|65535] "" XXX
+ SG_ DRIVER_BRAKE : 4|1@0+ (1,0) [0|1] "" XXX
+ SG_ COMPUTER_BRAKE : 10|1@0+ (1,0) [0|1] "" XXX
+ SG_ BRAKE_PRESSED : 0|1@1+ (1,0) [0|1] "" XXX
 
-BO_ 69 NEW_MSG_3: 8 XXX
- SG_ TURN_SIGNAL_LEVER : 16|8@1+ (1,0) [0|255] "" XXX
- SG_ CRUISE_CONTROL_LEVER : 7|8@0+ (1,0) [0|255] "" XXX
- SG_ NEW_SIGNAL_2 : 55|8@0+ (1,0) [0|255] "" XXX
- SG_ NEW_SIGNAL_1 : 63|8@0+ (1,0) [0|255] "" XXX
- SG_ STEERING_WHEEL_BUTTONS : 32|8@1+ (1,0) [0|255] "4 directional, 2 volume control, & 2 phone buttons" XXX
- SG_ MORE_STEERING_WHEELS_BUTTONS : 40|8@1+ (1,0) [0|255] "" XXX
+BO_ 69 DRIVER_CONTROLS: 8 XXX
+ SG_ COUNTER : 55|4@0+ (1,0) [0|15] "" XXX
+ SG_ STEERING_WHEEL_BUTTONS : 32|16@1+ (1,0) [0|255] "4 directional, 2 volume control & 2 phone buttons" XXX
+ SG_ LEFT_BLINKER : 16|1@0+ (1,0) [0|1] "" XXX
+ SG_ RIGHT_BLINKER : 17|1@0+ (1,0) [0|1] "" XXX
+ SG_ HIGHBEAM_TOGGLE : 18|1@0+ (1,0) [0|1] "" XXX
+ SG_ HIGHBEAM_MOMENTARY : 19|1@0+ (1,0) [0|1] "" XXX
+ SG_ CRUISE_CONTROL_CANCEL : 0|1@0+ (1,0) [0|1] "" XXX
+ SG_ CRUISE_CONTROL_RESUME : 1|1@0+ (1,0) [0|1] "" XXX
+ SG_ CRUISE_CONTROL_ACCEL_HIGH : 2|1@0+ (1,0) [0|1] "" XXX
+ SG_ CRUISE_CONTROL_DECEL_HIGH : 3|1@0+ (1,0) [0|1] "" XXX
+ SG_ CRUISE_CONTROL_ACCEL_LOW : 4|1@0+ (1,0) [0|1] "" XXX
+ SG_ CRUISE_CONTROL_DECEL_LOW : 5|1@0+ (1,0) [0|1] "" XXX
+ SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX
+ SG_ SET_ME_XFF : 15|8@0+ (1,0) [0|255] "" XXX
 
-BO_ 1 NEW_MSG_4: 8 XXX
- SG_ DOOR_LOCK_STATUS : 31|16@0+ (1,0) [0|65535] "" XXX
+BO_ 513 WHEEL_ENCODERS: 8 XXX
+ SG_ COUNTER : 55|4@0+ (1,0) [0|15] "" XXX
+ SG_ WHEEL_ENCODER_2 : 15|8@0+ (1,0) [0|255] "" XXX
+ SG_ WHEEL_ENCODER_3 : 23|8@0+ (1,0) [0|255] "" XXX
+ SG_ WHEEL_ENCODER_4 : 31|8@0+ (1,0) [0|255] "" XXX
+ SG_ CHECKSUM : 56|8@1+ (1,0) [0|255] "" XXX
+ SG_ WHEEL_ENCODER_1 : 7|8@0+ (1,0) [0|255] "" XXX
 
+BO_ 261 GAS_PEDAL: 8 XXX
+ SG_ COUNTER : 55|4@0+ (1,0) [0|15] "" XXX
+ SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX
+ SG_ ENGINE_RPM : 4|5@0+ (1,0) [0|255] "" XXX
+ SG_ GAS_PEDAL : 39|8@0+ (1,0) [0|255] "" XXX
+ SG_ COMBINED_GAS : 31|8@0+ (1,0) [0|255] "" XXX
 
+BO_ 643 DOOR_SENSORS: 8 XXX
+ SG_ BRAKE_PRESSED : 27|1@0+ (1,0) [0|1] "" XXX
+ SG_ DOOR_OPEN_FR : 3|1@1+ (1,0) [0|3] "" XXX
+ SG_ DOOR_OPEN_RL : 5|1@0+ (1,0) [0|3] "" XXX
+ SG_ DOOR_OPEN_RR : 7|1@0+ (1,0) [0|3] "" XXX
+ SG_ DOOR_OPEN_FL : 1|1@0+ (1,0) [0|1] "" XXX
+ SG_ DOOR_CLOSED_FL : 0|1@0+ (1,0) [0|1] "" XXX
+ SG_ DOOR_CLOSED_FR : 2|1@0+ (1,0) [0|1] "" XXX
+ SG_ DOOR_CLOSED_RL : 4|1@0+ (1,0) [0|1] "" XXX
+ SG_ DOOR_CLOSED_RR : 6|1@0+ (1,0) [0|1] "" XXX
 
+BO_ 885 SEATBELT_SENSORS: 8 XXX
+ SG_ SEATBELT_DRIVER_LATCHED : 16|1@0+ (1,0) [0|1] "" XXX
+ SG_ SEATBELT_PASSENGER_LATCHED : 18|1@0+ (1,0) [0|1] "" XXX
 
-CM_ SG_ 5 BRAKE_PRESSED "appears to be boolean (brake pressed)";
-CM_ SG_ 69 MORE_STEERING_WHEELS_BUTTONS "back, ok, voice assistance, and mute buttons";
+BO_ 257 CRUISE_CONTROL: 8 XXX
+ SG_ COUNTER : 55|4@0+ (1,0) [0|15] "" XXX
+ SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX
+ SG_ NEW_SIGNAL_1 : 6|1@0+ (1,0) [0|255] "" XXX
+ SG_ CRUISE_DISABLED : 23|1@0+ (1,0) [0|1] "" XXX
+ SG_ SET_ME_X002 : 39|8@0+ (1,0) [0|255] "" XXX
+ SG_ SET_ME_X00 : 31|8@0+ (1,0) [0|255] "" XXX
+ SG_ SET_ME_X003 : 47|8@0+ (1,0) [0|255] "" XXX
+ SG_ SET_ME_1 : 5|1@0+ (1,0) [0|1] "" XXX
+ SG_ CRUISE_ACCELERATING : 22|1@0+ (1,0) [0|1] "" XXX
+ SG_ LONGITUDINAL_ACCEL_REQUEST : 15|8@0- (1,0) [0|127] "" XXX
+
+BO_ 260 CRUISE_CONTROL2: 8 XXX
+ SG_ COUNTER : 55|4@0+ (1,0) [0|15] "" XXX
+ SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX
+ SG_ SET_ME_X00 : 47|8@0+ (1,0) [0|255] "" XXX
+ SG_ SET_ME_XFF : 31|8@0+ (1,0) [0|65535] "" XXX
+ SG_ SET_ME_X02 : 23|8@0+ (1,0) [0|255] "" XXX
+ SG_ SET_ME_XFF2 : 39|8@0+ (1,0) [0|255] "" XXX
+ SG_ NEW_SIGNAL_1 : 7|4@0+ (1,0) [0|255] "" XXX
+
+BO_ 14 STEER_TORQUE: 8 XXX
+ SG_ STEER_TORQUE : 15|8@0+ (1,0) [0|255] "" XXX
+ SG_ COUNTER : 55|4@0+ (1,0) [0|15] "" XXX
+ SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX
+
+BO_ 888 CRUISE_CONTROL3: 8 XXX
+ SG_ NEW_SIGNAL_2 : 7|1@0+ (1,0) [0|1] "" XXX
+ SG_ NEW_SIGNAL_1 : 38|1@0+ (1,0) [0|1] "" XXX
+ SG_ NEW_SIGNAL_5 : 32|1@0+ (1,0) [0|1] "" XXX
+ SG_ CRUISE_DISABLED : 36|1@0+ (1,0) [0|1] "" XXX
+ SG_ CRUISE_ENABLED : 34|1@0+ (1,0) [0|1] "" XXX
+ SG_ SET_ME_X003 : 47|8@0+ (1,0) [0|255] "" XXX
+ SG_ SET_ME_X004 : 63|8@0+ (1,0) [0|255] "" XXX
+ SG_ SET_ME_X002 : 31|8@0+ (1,0) [0|255] "" XXX
+ SG_ SET_ME_X00 : 23|8@0+ (1,0) [0|255] "" XXX
+ SG_ CRUISE_SET_SPEED : 15|8@0+ (1,0) [0|63] "mph" XXX
+ SG_ CRUISE_SPEED_CHANGE : 55|1@0+ (1,0) [0|1] "" XXX
+
+BO_ 307 POWER_SEATS: 8 XXX
+ SG_ DRIVER_FORWARD : 0|1@0+ (1,0) [0|1] "" XXX
+ SG_ DRIVER_BACK : 1|1@0+ (1,0) [0|1] "" XXX
+
+BO_ 109 GEAR_LEVER: 8 XXX
+ SG_ PARK_BUTTON : 12|1@0+ (1,0) [0|1] "" XXX
+ SG_ NEUTRAL_UP : 9|1@0+ (1,0) [0|1] "" XXX
+ SG_ NEUTRAL_DOWN : 10|1@0+ (1,0) [0|1] "" XXX
+ SG_ DRIVE : 11|1@0+ (1,0) [0|1] "" XXX
+ SG_ REVERSE : 8|1@0+ (1,0) [0|1] "" XXX
+ SG_ COUNTER : 23|4@0+ (1,0) [0|15] "" XXX
+ SG_ CHECKSUM : 31|8@0+ (1,0) [0|255] "" XXX
+
+BO_ 115 GEAR_PACKET: 8 XXX
+ SG_ GEAR : 0|4@1+ (1,0) [0|15] "" XXX
+
+BO_ 581 IGNITION: 8 XXX
+
+BO_ 515 WHEEL_SPEEDS: 8 XXX
+ SG_ WHEEL_MOVING_FR : 22|1@1+ (1,0) [0|15] "" XXX
+ SG_ WHEEL_MOVING_RL : 38|1@0+ (1,0) [0|1] "" XXX
+ SG_ WHEEL_MOVING_FL : 6|1@0+ (1,0) [0|1] "" XXX
+ SG_ WHEEL_MOVING_RR : 54|1@0+ (1,0) [0|1] "" XXX
+ SG_ WHEEL_SPEED_FL : 2|11@0+ (0.0375,0) [0|255] "mph" XXX
+ SG_ WHEEL_SPEED_FR : 18|11@0+ (0.0375,0) [0|255] "mph" XXX
+ SG_ WHEEL_SPEED_RL : 34|11@0+ (0.0375,0) [0|255] "mph" XXX
+ SG_ WHEEL_SPEED_RR : 50|11@0+ (0.0375,0) [0|255] "mph" XXX
+
+
+
+
+CM_ SG_ 3 STEER_DIRECTION "0 = left, 1 = right";
+CM_ SG_ 5 BRAKE_POSITION "computer and driver";
+CM_ SG_ 5 BRAKE_PRESSED "computer and driver";
+CM_ SG_ 261 GAS_PEDAL "user gas input";
+CM_ SG_ 261 COMBINED_GAS "computer and driver gas";
+CM_ SG_ 257 CRUISE_ACCELERATING "";
\ No newline at end of file
diff --git a/panda/VERSION b/panda/VERSION
index 89fb16f883f364..261f3475721dd3 100644
--- a/panda/VERSION
+++ b/panda/VERSION
@@ -1 +1 @@
-v1.1.4
\ No newline at end of file
+v1.1.5
\ No newline at end of file
diff --git a/panda/board/safety/safety_gm.h b/panda/board/safety/safety_gm.h
index 0aa94244446c80..f35b26b4ef84d3 100644
--- a/panda/board/safety/safety_gm.h
+++ b/panda/board/safety/safety_gm.h
@@ -8,7 +8,7 @@
 //      brake rising edge
 //      brake > 0mph
 
-const int GM_MAX_STEER = 255;
+const int GM_MAX_STEER = 300;
 const int GM_MAX_RT_DELTA = 128;          // max delta torque allowed for real time checks
 const int32_t GM_RT_INTERVAL = 250000;    // 250ms between real time checks
 const int GM_MAX_RATE_UP = 7;
diff --git a/panda/tests/safety/test_gm.py b/panda/tests/safety/test_gm.py
index 346fc07381ba60..299de5a7e9ff51 100644
--- a/panda/tests/safety/test_gm.py
+++ b/panda/tests/safety/test_gm.py
@@ -5,7 +5,7 @@
 
 MAX_RATE_UP = 7
 MAX_RATE_DOWN = 17
-MAX_STEER = 255
+MAX_STEER = 300
 MAX_BRAKE = 350
 MAX_GAS = 3072
 MAX_REGEN = 1404
diff --git a/selfdrive/boardd/Makefile b/selfdrive/boardd/Makefile
index 1573304e673498..893edde545b848 100644
--- a/selfdrive/boardd/Makefile
+++ b/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)
-CXXFLAGS = -std=c++11 -g -fPIC -I../../ -O2 $(WARN_FLAGS)
+CFLAGS = -std=gnu11 -g -fPIC -I../ -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 \
diff --git a/selfdrive/car/gm/carcontroller.py b/selfdrive/car/gm/carcontroller.py
index 08039323067c2f..199b6a5c3268e6 100644
--- a/selfdrive/car/gm/carcontroller.py
+++ b/selfdrive/car/gm/carcontroller.py
@@ -4,14 +4,14 @@
 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
 
 
 class CarControllerParams():
   def __init__(self, car_fingerprint):
     if car_fingerprint == CAR.VOLT:
-      self.STEER_MAX = 255
+      self.STEER_MAX = 300
       self.STEER_STEP = 2              # how often we update the steer cmd
       self.STEER_DELTA_UP = 7          # ~0.75s time to peak torque (255/50hz/0.75s)
       self.STEER_DELTA_DOWN = 17       # ~0.3s from peak torque to zero
@@ -29,11 +29,11 @@ def __init__(self, car_fingerprint):
     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 @@ def update(self, sendcan, enabled, CS, frame, actuators, \
       return
 
     P = self.params
-
     # Send CAN commands.
     can_sends = []
     canbus = self.canbus
@@ -91,14 +90,11 @@ def update(self, sendcan, enabled, CS, frame, actuators, \
     ### STEER ###
 
     if (frame % P.STEER_STEP) == 0:
-      final_steer = actuators.steer if enabled else 0.
-      apply_steer = final_steer * P.STEER_MAX
-
-      apply_steer = apply_std_steer_torque_limits(apply_steer, self.apply_steer_last, CS.steer_torque_driver, P)
-
       lkas_enabled = enabled and not CS.steer_not_allowed and CS.v_ego > 3.
-
-      if not lkas_enabled:
+      if lkas_enabled:
+        apply_steer = actuators.steer * P.STEER_MAX
+        apply_steer = apply_std_steer_torque_limits(apply_steer, self.apply_steer_last, CS.steer_torque_driver, P)
+      else:
         apply_steer = 0
 
       self.apply_steer_last = apply_steer
@@ -115,7 +111,7 @@ def update(self, sendcan, enabled, CS, frame, actuators, \
 
     if self.car_fingerprint == CAR.VOLT:
       # no output if not enabled, but keep sending keepalive messages
-      # threat pedals as one
+      # treat pedals as one
       final_pedal = actuators.gas - actuators.brake
 
       # *** apply pedal hysteresis ***
@@ -123,7 +119,8 @@ def update(self, sendcan, enabled, CS, frame, actuators, \
         final_pedal, self.pedal_steady)
 
       if not enabled:
-        apply_gas = P.MAX_ACC_REGEN  # TODO: do we really need to send max regen when not enabled?
+        # Stock ECU sends max regen when not enabled.
+        apply_gas = P.MAX_ACC_REGEN
         apply_brake = 0
       else:
         apply_gas = int(round(interp(final_pedal, P.GAS_LOOKUP_BP, P.GAS_LOOKUP_V)))
@@ -133,12 +130,18 @@ def update(self, sendcan, enabled, CS, frame, actuators, \
       if (frame % 4) == 0:
         idx = (frame / 4) % 4
 
-        at_full_stop = enabled and CS.standstill
-        near_stop = enabled and (CS.v_ego < P.NEAR_STOP_BRAKE_PHASE)
+        car_stopping = apply_gas < P.ZERO_GAS
+        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
-        can_sends.append(gmcan.create_gas_regen_command(self.packer_pt, canbus.powertrain, apply_gas, idx, enabled, at_full_stop))
+        # Auto-resume from full stop by resetting ACC control
+        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:
diff --git a/selfdrive/car/gm/gmcan.py b/selfdrive/car/gm/gmcan.py
index 47b0d8b38dfad4..6db69bad592768 100644
--- a/selfdrive/car/gm/gmcan.py
+++ b/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:
+  mode = 0x1
+  if apply_brake > 0:
     mode = 0xa
 
-    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
+  if near_stop:
+    mode = 0xb
+
+  if at_full_stop:
+    mode = 0xd
 
   brake = (0x1000 - apply_brake) & 0xfff
   checksum = (0x10000 - (mode << 12) - brake - idx) & 0xffff
diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py
index a9530d65ae80a9..f7fb42a391f089 100755
--- a/selfdrive/car/gm/interface.py
+++ b/selfdrive/car/gm/interface.py
@@ -4,7 +4,7 @@
 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 @@ def __init__(self):
     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 @@ def get_params(candidate, fingerprint):
     # 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 @@ def update(self, c):
     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 @@ def update(self, c):
         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:
@@ -298,7 +292,7 @@ def update(self, c):
         events.append(create_event('pcmEnable', [ET.ENABLE]))
       if not self.CS.acc_active:
         events.append(create_event('pcmDisable', [ET.USER_DISABLE]))
-  
+
     ret.events = events
 
     # update previous brake/gas pressed
diff --git a/selfdrive/car/gm/values.py b/selfdrive/car/gm/values.py
index b69109f356735d..62529ed6d73578 100644
--- a/selfdrive/car/gm/values.py
+++ b/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 @@ def parse_gear_shifter(can_gear):
 
 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'),
diff --git a/selfdrive/car/honda/carstate.py b/selfdrive/car/honda/carstate.py
index 5bea72bf74556a..de50c277fb7144 100644
--- a/selfdrive/car/honda/carstate.py
+++ b/selfdrive/car/honda/carstate.py
@@ -76,7 +76,7 @@ def get_can_signals(CP):
 
   if CP.radarOffCan:
     # Civic is only bosch to use the same brake message as other hondas.
-    if CP.carFingerprint != CAR.CIVIC_HATCH:
+    if CP.carFingerprint not in (CAR.ACCORDH, CAR.CIVIC_HATCH):
       signals += [("BRAKE_PRESSED", "BRAKE_MODULE", 0)]
       checks += [("BRAKE_MODULE", 50)]
     signals += [("CAR_GAS", "GAS_PEDAL_2", 0),
@@ -91,7 +91,7 @@ def get_can_signals(CP):
                 ("CRUISE_SPEED_OFFSET", "CRUISE_PARAMS", 0)]
     checks += [("CRUISE_PARAMS", 50)]
 
-  if CP.carFingerprint in (CAR.ACCORD, CAR.ACCORD_15):
+  if CP.carFingerprint in (CAR.ACCORD, CAR.ACCORD_15, CAR.ACCORDH):
     signals += [("DRIVERS_DOOR_OPEN", "SCM_FEEDBACK", 1)]
   else:
     signals += [("DOOR_OPEN_FL", "DOORS_STATUS", 1),
@@ -213,7 +213,7 @@ def update(self, cp):
 
     # ******************* parse out can *******************
 
-    if self.CP.carFingerprint in (CAR.ACCORD, CAR.ACCORD_15): # TODO: find wheels moving bit in dbc
+    if self.CP.carFingerprint in (CAR.ACCORD, CAR.ACCORD_15, CAR.ACCORDH): # TODO: find wheels moving bit in dbc
       self.standstill = cp.vl["ENGINE_DATA"]['XMISSION_SPEED'] < 0.1
       self.door_all_closed = not cp.vl["SCM_FEEDBACK"]['DRIVERS_DOOR_OPEN']
     else:
@@ -268,7 +268,7 @@ def update(self, cp):
     self.left_blinker_on = cp.vl["SCM_FEEDBACK"]['LEFT_BLINKER']
     self.right_blinker_on = cp.vl["SCM_FEEDBACK"]['RIGHT_BLINKER']
 
-    if self.CP.carFingerprint in (CAR.CIVIC, CAR.ODYSSEY, CAR.CRV_5G, CAR.ACCORD, CAR.ACCORD_15, CAR.CIVIC_HATCH):
+    if self.CP.carFingerprint in (CAR.CIVIC, CAR.ODYSSEY, CAR.CRV_5G, CAR.ACCORD, CAR.ACCORD_15, CAR.ACCORDH, CAR.CIVIC_HATCH):
       self.park_brake = cp.vl["EPB_STATUS"]['EPB_STATE'] != 0
       self.brake_hold = cp.vl["VSA_STATUS"]['BRAKE_HOLD_ACTIVE']
       self.main_on = cp.vl["SCM_FEEDBACK"]['MAIN_ON']
@@ -295,7 +295,7 @@ def update(self, cp):
     if self.CP.radarOffCan:
       self.stopped = cp.vl["ACC_HUD"]['CRUISE_SPEED'] == 252.
       self.cruise_speed_offset = calc_cruise_offset(0, self.v_ego)
-      if self.CP.carFingerprint == CAR.CIVIC_HATCH:
+      if self.CP.carFingerprint in (CAR.CIVIC_HATCH, CAR.ACCORDH):
         self.brake_switch = cp.vl["POWERTRAIN_DATA"]['BRAKE_SWITCH']
         self.brake_pressed = cp.vl["POWERTRAIN_DATA"]['BRAKE_PRESSED'] or \
                           (self.brake_switch and self.brake_switch_prev and \
diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py
index 6baa3e5aeff8c1..2bff2e74601b6d 100755
--- a/selfdrive/car/honda/interface.py
+++ b/selfdrive/car/honda/interface.py
@@ -204,9 +204,10 @@ def get_params(candidate, fingerprint):
       ret.longitudinalKiBP = [0., 35.]
       ret.longitudinalKiV = [0.18, 0.12]
 
-    elif candidate in (CAR.ACCORD, CAR.ACCORD_15):
+    elif candidate in (CAR.ACCORD, CAR.ACCORD_15, CAR.ACCORDH):
       stop_and_go = True
-      ret.safetyParam = 1 # Accord and CRV 5G use an alternate user brake msg
+      if not candidate == CAR.ACCORDH: # Hybrid uses same brake msg as hatch
+        ret.safetyParam = 1 # Accord and CRV 5G use an alternate user brake msg
       ret.mass = 3279. * CV.LB_TO_KG + std_cargo
       ret.wheelbase = 2.83
       ret.centerToFront = ret.wheelbase * 0.39
diff --git a/selfdrive/car/honda/values.py b/selfdrive/car/honda/values.py
index c2bb9cc6afe290..eec1a7e0c69b33 100644
--- a/selfdrive/car/honda/values.py
+++ b/selfdrive/car/honda/values.py
@@ -7,7 +7,6 @@ class CruiseButtons:
   CANCEL      = 2
   MAIN        = 1
 
-
 #car chimes: enumeration from dbc file. Chimes are for alerts and warnings
 class CM:
   MUTE = 0
@@ -16,7 +15,6 @@ class CM:
   REPEATED = 1
   CONTINUOUS = 2
 
-
 #car beepss: enumeration from dbc file. Beeps are for activ and deactiv
 class BP:
   MUTE = 0
@@ -35,10 +33,10 @@ class AH:
   SEATBELT       = [5, 5]
   SPEED_TOO_HIGH = [6, 8]
 
-
 class CAR:
   ACCORD = "HONDA ACCORD 2018 SPORT 2T"
   ACCORD_15 = "HONDA ACCORD 2018 LX 1.5T"
+  ACCORDH = "HONDA ACCORD 2018 HYBRID TOURING"
   CIVIC = "HONDA CIVIC 2016 TOURING"
   CIVIC_HATCH = "HONDA CIVIC HATCHBACK 2017 EX"
   ACURA_ILX = "ACURA ILX 2016 ACURAWATCH PLUS"
@@ -50,14 +48,16 @@ class CAR:
   PILOT_2019 = "HONDA PILOT 2019 ELITE"
   RIDGELINE = "HONDA RIDGELINE 2017 BLACK EDITION"
 
-
 FINGERPRINTS = {
   CAR.ACCORD: [{
-    148: 8, 228: 5, 304: 8, 330: 8, 344: 8, 380: 8, 387: 8, 388: 8, 399: 7, 419: 8, 420: 8, 427: 3, 432: 7, 441: 5, 446: 3, 450: 8, 464: 8, 477: 8, 479: 8, 495: 8, 525: 8, 545: 6, 662: 4, 773: 7, 777: 8, 780: 8, 804: 8, 806: 8, 808: 8, 829: 5, 862: 8, 884: 8, 891: 8, 927: 8, 929: 8, 1302: 8, 1600: 5, 1601: 8, 1652: 8
+    148: 8, 228: 5, 304: 8, 330: 8, 344: 8, 380: 8, 399: 7, 419: 8, 420: 8, 427: 3, 432: 7, 441: 5, 446: 3, 450: 8, 464: 8, 477: 8, 479: 8, 495: 8, 545: 6, 662: 4, 773: 7, 777: 8, 780: 8, 804: 8, 806: 8, 808: 8, 829: 5, 862: 8, 884: 8, 891: 8, 927: 8, 929: 8, 1302: 8, 1600: 5, 1601: 8, 1652: 8
   }],
   CAR.ACCORD_15: [{
     148: 8, 228: 5, 304: 8, 330: 8, 344: 8, 380: 8, 399: 7, 401: 8, 420: 8, 427: 3, 432: 7, 441: 5, 446: 3, 450: 8, 464: 8, 477: 8, 479: 8, 495: 8, 545: 6, 662: 4, 773: 7, 777: 8, 780: 8, 804: 8, 806: 8, 808: 8, 829: 5, 862: 8, 884: 8, 891: 8, 927: 8, 929: 8, 1302: 8, 1600: 5, 1601: 8, 1652: 8
   }],
+  CAR.ACCORDH: [{
+    148: 8, 228: 5, 304: 8, 330: 8, 344: 8, 380: 8, 387: 8, 388: 8, 399: 7, 419: 8, 420: 8, 427: 3, 432: 7, 441: 5, 450: 8, 464: 8, 477: 8, 479: 8, 495: 8, 525: 8, 545: 6, 662: 4, 773: 7, 777: 8, 780: 8, 804: 8, 806: 8, 808: 8, 829: 5, 862: 8, 884: 8, 891: 8, 927: 8, 929: 8, 1302: 8, 1600: 5, 1601: 8, 1652: 8
+  }],
   CAR.ACURA_ILX: [{
     57: 3, 145: 8, 228: 5, 304: 8, 316: 8, 342: 6, 344: 8, 380: 8, 398: 3, 399: 7, 419: 8, 420: 8, 422: 8, 428: 8, 432: 7, 464: 8, 476: 4, 490: 8, 506: 8, 512: 6, 513: 6, 542: 7, 545: 4, 597: 8, 660: 8, 773: 7, 777: 8, 780: 8, 800: 8, 804: 8, 808: 8, 819: 7, 821: 5, 829: 5, 882: 2, 884: 7, 887: 8, 888: 8, 892: 8, 923: 2, 929: 4, 983: 8, 985: 3, 1024: 5, 1027: 5, 1029: 8, 1030: 5, 1034: 5, 1036: 8, 1039: 8, 1057: 5, 1064: 7, 1108: 8, 1365: 5,
   }],
@@ -98,10 +98,10 @@ class CAR:
   }]
 }
 
-
 DBC = {
   CAR.ACCORD: dbc_dict('honda_accord_s2t_2018_can_generated', None),
   CAR.ACCORD_15: dbc_dict('honda_accord_lx15t_2018_can_generated', None),
+  CAR.ACCORDH: dbc_dict('honda_accord_s2t_2018_can_generated', None),
   CAR.ACURA_ILX: dbc_dict('acura_ilx_2016_can_generated', 'acura_ilx_2016_nidec'),
   CAR.ACURA_RDX: dbc_dict('acura_rdx_2018_can_generated', 'acura_ilx_2016_nidec'),
   CAR.CIVIC: dbc_dict('honda_civic_touring_2016_can_generated', 'acura_ilx_2016_nidec'),
@@ -114,10 +114,10 @@ class CAR:
   CAR.RIDGELINE: dbc_dict('honda_ridgeline_black_edition_2017_can_generated', 'acura_ilx_2016_nidec'),
 }
 
-
 STEER_THRESHOLD = {
   CAR.ACCORD: 1200,
   CAR.ACCORD_15: 1200,
+  CAR.ACCORDH: 1200,
   CAR.ACURA_ILX: 1200,
   CAR.ACURA_RDX: 400,
   CAR.CIVIC: 1200,
@@ -133,6 +133,7 @@ class CAR:
 SPEED_FACTOR = {
   CAR.ACCORD: 1.,
   CAR.ACCORD_15: 1.,
+  CAR.ACCORDH: 1.,
   CAR.ACURA_ILX: 1.,
   CAR.ACURA_RDX: 1.,
   CAR.CIVIC: 1.,
@@ -159,4 +160,4 @@ class CAR:
 }
 
 # TODO: get these from dbc file
-HONDA_BOSCH = [CAR.ACCORD, CAR.ACCORD_15, CAR.CIVIC_HATCH, CAR.CRV_5G]
+HONDA_BOSCH = [CAR.ACCORD, CAR.ACCORD_15, CAR.ACCORDH, CAR.CIVIC_HATCH, CAR.CRV_5G]
diff --git a/selfdrive/car/hyundai/carstate.py b/selfdrive/car/hyundai/carstate.py
index 4bd7c492475806..4714644957bb3d 100644
--- a/selfdrive/car/hyundai/carstate.py
+++ b/selfdrive/car/hyundai/carstate.py
@@ -17,7 +17,7 @@ def get_can_parser(CP):
     ("YAW_RATE", "ESP12", 0),
 
     ("CF_Gway_DrvSeatBeltInd", "CGW4", 1),
-    
+
     ("CF_Gway_DrvSeatBeltSw", "CGW1", 0),
     ("CF_Gway_TSigLHSw", "CGW1", 0),
     ("CF_Gway_TurnSigLh", "CGW1", 0),
diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py
index aca2ee570c7cb6..5f9d75c94b1287 100644
--- a/selfdrive/car/hyundai/interface.py
+++ b/selfdrive/car/hyundai/interface.py
@@ -70,13 +70,18 @@ def get_params(candidate, fingerprint):
     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 @@ def get_params(candidate, fingerprint):
     ret.longitudinalKpV = [0.]
     ret.longitudinalKiBP = [0.]
     ret.longitudinalKiV = [0.]
-    tire_stiffness_factor = 1.
 
     ret.centerToFront = ret.wheelbase * 0.4
 
diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py
index 6d939f639efdd6..f5ef7823c530bb 100644
--- a/selfdrive/car/toyota/carcontroller.py
+++ b/selfdrive/car/toyota/carcontroller.py
@@ -162,6 +162,9 @@ def update(self, sendcan, enabled, CS, frame, actuators,
     # 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)
@@ -211,12 +214,12 @@ def update(self, sendcan, enabled, CS, frame, actuators,
     # 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, 
+      can_sends.append(create_ipas_steer_command(self.packer, apply_angle, self.steer_angle_enabled,
                                                  ECU.APGS in self.fake_ecus))
     elif ECU.APGS in self.fake_ecus:
       can_sends.append(create_ipas_steer_command(self.packer, 0, 0, True))
diff --git a/selfdrive/car/toyota/toyotacan.py b/selfdrive/car/toyota/toyotacan.py
index 150167b9e6a8da..2b37b85da4bcbb 100644
--- a/selfdrive/car/toyota/toyotacan.py
+++ b/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,
diff --git a/selfdrive/car/toyota/values.py b/selfdrive/car/toyota/values.py
index 0fb407abe5aa39..1afab157ee7702 100644
--- a/selfdrive/car/toyota/values.py
+++ b/selfdrive/car/toyota/values.py
@@ -109,12 +109,16 @@ def check_ecu_msgs(fingerprint, ecu):
     36: 8, 37: 8, 170: 8, 180: 8, 186: 4, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 550: 8, 552: 4, 562: 6, 608: 8, 610: 8, 643: 7, 705: 8, 740: 5, 800: 8, 810: 2, 812: 8, 830: 7, 835: 8, 836: 8, 869: 7, 870: 7, 871: 2, 921: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 976: 1, 1017: 8, 1020: 8, 1021: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1059: 1, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1235: 8, 1279: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1595: 8, 1745: 8, 1779: 8
   }],
   CAR.CHRH: [{
-    36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 8, 643: 7, 713: 8, 740: 5, 800: 8, 810: 2, 812: 8, 829: 2, 830: 7, 835: 8, 836: 8, 845: 5, 869: 7, 870: 7, 871: 2, 898: 8, 900: 6, 902: 6, 905: 8, 913: 8, 921: 8, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 975: 5, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1014: 8, 1017: 8, 1020: 8, 1021: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1057: 8,1059: 1, 1071: 8, 1076: 8, 1077: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1235: 8, 1237: 8, 1279: 8, 1552: 8, 1553: 8, 1556:8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1595: 8, 1745: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
+    36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 8, 614: 8, 643: 7, 658: 8, 713: 8, 740: 5, 800: 8, 810: 2, 812: 8, 814: 8, 829: 2, 830: 7, 835: 8, 836: 8, 845: 5, 869: 7, 870: 7, 871: 2, 898: 8, 900: 6, 902: 6, 905: 8, 913: 8, 918: 8, 921: 8, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 975: 5, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1014: 8, 1017: 8, 1020: 8, 1021: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1071: 8, 1076: 8, 1077: 8, 1082: 8, 1083: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1175: 8, 1228: 8, 1235: 8, 1237: 8, 1279: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1595: 8, 1745: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
   }],
   CAR.CAMRY: [
   #XLE and LE
   {
     36: 8, 37: 8, 119: 6, 170: 8, 180: 8, 186: 4, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 550: 8, 552: 4, 562: 6, 608: 8, 610: 8, 643: 7, 658: 8, 705: 8, 728: 8, 740: 5, 761: 8, 764: 8, 800: 8, 810: 2, 812: 8, 814: 8, 818: 8, 822: 8, 824: 8, 830: 7, 835: 8, 836: 8, 869: 7, 870: 7, 871: 2, 888: 8, 889: 8, 891: 8, 898: 8, 900: 6, 902: 6, 905: 8, 918: 8, 921: 8, 933: 8, 934: 8, 935: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 976: 1, 983: 8, 984: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1011: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1059: 1, 1076: 8, 1077: 8, 1082: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1228: 8, 1235: 8, 1237: 8, 1263: 8, 1264: 8, 1279: 8, 1412: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1595: 8, 1745: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1808: 8, 1816: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
+  },
+  #XSE and SE
+  {
+    36: 8, 37: 8, 114: 5, 119: 6, 120: 4, 170: 8, 180: 8, 186: 4, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 550: 8, 552: 4, 562: 6, 608: 8, 610: 8, 643: 7, 658: 8, 705: 8, 728: 8, 761: 8, 764: 8, 800: 8, 810: 2, 812: 8, 814: 8, 818: 8, 822: 8, 824: 8, 830: 7, 835: 8, 836: 8, 869: 7, 870: 7, 888: 8, 889: 8, 891: 8, 898: 8, 900: 6, 902: 6, 905: 8, 918: 8, 921: 8, 933: 8, 934: 8, 935: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 976: 1, 983: 8, 984: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1011: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1056: 8, 1059: 1, 1076: 8, 1077: 8, 1082: 8, 1114: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1228: 8, 1237: 8, 1263: 8, 1264: 8, 1279: 8, 1412: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1595: 8, 1745: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1808: 8, 1816: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
   }],
   CAR.CAMRYH: [
   #LE
diff --git a/selfdrive/common/ipc.c b/selfdrive/common/ipc.c
new file mode 100644
index 00000000000000..8d391074786815
--- /dev/null
+++ b/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;
+  }
+}
diff --git a/selfdrive/common/ipc.h b/selfdrive/common/ipc.h
new file mode 100644
index 00000000000000..7ebfd716468298
--- /dev/null
+++ b/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
\ No newline at end of file
diff --git a/selfdrive/common/params.cc b/selfdrive/common/params.cc
index 02d91efcead853..7bbcf5fad11c60 100644
--- a/selfdrive/common/params.cc
+++ b/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 "selfdrive/common/utilpp.h"
+#include "common/util.h"
+#include "common/utilpp.h"
 
 namespace {
 
diff --git a/selfdrive/common/utilpp.h b/selfdrive/common/utilpp.h
index bc1b2fd267d966..e374c5c256f772 100644
--- a/selfdrive/common/utilpp.h
+++ b/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,13 +52,13 @@ inline std::string dir_name(std::string const & path) {
 }
 
 inline std::string readlink(std::string path) {
-    char buff[PATH_MAX];
-    ssize_t len = ::readlink(path.c_str(), buff, sizeof(buff)-1);
-    if (len != -1) {
-      buff[len] = '\0';
-      return std::string(buff);
-    }
-    return "";
+  char buff[4096];
+  ssize_t len = ::readlink(path.c_str(), buff, sizeof(buff)-1);
+  if (len != -1) {
+    buff[len] = '\0';
+    return std::string(buff);
+  }
+  return "";
 }
 
 }
diff --git a/selfdrive/common/version.h b/selfdrive/common/version.h
index 1193b7af4909e9..a8edf68845b620 100644
--- a/selfdrive/common/version.h
+++ b/selfdrive/common/version.h
@@ -1 +1 @@
-#define COMMA_VERSION "0.5.3-release"
+#define COMMA_VERSION "0.5.4-release"
diff --git a/selfdrive/common/visionimg.cc b/selfdrive/common/visionimg.cc
index 45bc7bfc4a5be6..8179166e1c7759 100644
--- a/selfdrive/common/visionimg.cc
+++ b/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) {
-
-  int aligned_w = 0, aligned_h = 0;
+void visionimg_compute_aligned_width_and_height(int width, int height, int *aligned_w, int *aligned_h) {
 #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;
diff --git a/selfdrive/common/visionimg.h b/selfdrive/common/visionimg.h
index 1a98b55bc7579a..9fabb6054f6dd3 100644
--- a/selfdrive/common/visionimg.h
+++ b/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
diff --git a/selfdrive/common/visionipc.c b/selfdrive/common/visionipc.c
index d56fbdd7736d9f..314f7d0a55c712 100644
--- a/selfdrive/common/visionipc.c
+++ b/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;
-
-  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;
+  return ipc_connect(VIPC_SOCKET_PATH);
 }
 
-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,
diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py
index c598167928b12d..ec9d108e53062a 100755
--- a/selfdrive/controls/controlsd.py
+++ b/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,
-                state, mismatch_counter, params):
+                poller, cal_status, cal_perc, overtemp, free_space, low_battery,
+                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 ***
 
@@ -292,10 +299,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
@@ -478,8 +488,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)
 
@@ -500,8 +512,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,
-      driver_monitor, gps_location, poller, cal_status, overtemp, free_space, driver_status, geofence, state, mismatch_counter, params)
+    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, cal_perc, overtemp, free_space, low_battery, driver_status, geofence, state, mismatch_counter, params)
     prof.checkpoint("Sample")
 
     # define plan
@@ -516,7 +528,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
diff --git a/selfdrive/controls/lib/alertmanager.py b/selfdrive/controls/lib/alertmanager.py
index f8a43d6a79f2ae..4c591dd768a992 100644
--- a/selfdrive/controls/lib/alertmanager.py
+++ b/selfdrive/controls/lib/alertmanager.py
@@ -6,11 +6,12 @@
 
 # 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
@@ -161,7 +162,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",
@@ -248,6 +249,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",
@@ -267,7 +274,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,
@@ -291,6 +298,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",
@@ -330,13 +343,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.),
 
@@ -396,7 +409,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,
@@ -458,13 +471,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.),
 
@@ -498,23 +511,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),
   }
 
@@ -524,10 +543,11 @@ def __init__(self):
   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
diff --git a/selfdrive/controls/lib/driver_monitor.py b/selfdrive/controls/lib/driver_monitor.py
index 23e247ed7c3c6f..10e0e0be66131b 100644
--- a/selfdrive/controls/lib/driver_monitor.py
+++ b/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 @@
 _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_K = 2 * np.pi * _DISTRACTED_FILTER_F * _DTM / (1 + 2 * np.pi * _DISTRACTED_FILTER_F * _DTM)
-_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)
+_DISTRACTED_FILTER_TS = 0.25 # 0.6Hz
+_VARIANCE_FILTER_TS = 20.    # 0.008Hz
 
 
 class _DriverPose():
@@ -47,15 +46,15 @@ def __init__(self, monitor_on=False):
     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.variance_level = 0.
+    self.driver_distraction_filter.x = 0.
+    self.variance_filter.x = 0.
     self.monitor_valid = True
 
   def _set_timers(self):
@@ -90,11 +89,9 @@ def get_pose(self, driver_monitoring, params):
     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 + \
-                                    _DISTRACTED_FILTER_K * self.driver_distracted
+    self.driver_distraction_filter.update(self.driver_distracted)
     self.variance_high = driver_monitoring.std > _STD_THRESHOLD
-    self.variance_level = (1. - _VARIANCE_FILTER_K) * self.variance_level + \
-                          _VARIANCE_FILTER_K * self.variance_high
+    self.variance_filter.update(self.variance_high)
 
     monitor_param_on_prev = self.monitor_param_on
     monitor_valid_prev = self.monitor_valid
@@ -105,7 +102,7 @@ def get_pose(self, driver_monitoring, params):
       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 @@ def get_pose(self, driver_monitoring, params):
 
   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 @@ def update(self, events, driver_engaged, ctrl_active, standstill):
 
 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):
-    ds.update([], False, True, True)
-    print(ds.awareness, ds.driver_distracted, ds.driver_distraction_level)
+  for i in range(10):
+    ds.update([], False, True, False)
+    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)
 
diff --git a/selfdrive/controls/lib/lateral_mpc/Makefile b/selfdrive/controls/lib/lateral_mpc/Makefile
index becef563e5ea53..65ebd876d28668 100644
--- a/selfdrive/controls/lib/lateral_mpc/Makefile
+++ b/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 \
-  qp/Constraints.o \
-  qp/CyclingManager.o \
-  qp/Indexlist.o \
-  qp/MessageHandling.o \
-  qp/QProblem.o \
-  qp/QProblemB.o \
-  qp/SubjectTo.o \
-  qp/Utils.o \
-  qp/EXTRAS/SolutionAnalysis.o \
-	mpc_export/acado_qpoases_interface.o \
-  mpc_export/acado_integrator.o \
-  mpc_export/acado_solver.o \
-  mpc_export/acado_auxiliary_functions.o \
-  mpc.o
+  lib_qp/Bounds.o \
+  lib_qp/Constraints.o \
+  lib_qp/CyclingManager.o \
+  lib_qp/Indexlist.o \
+  lib_qp/MessageHandling.o \
+  lib_qp/QProblem.o \
+  lib_qp/QProblemB.o \
+  lib_qp/SubjectTo.o \
+  lib_qp/Utils.o \
+  lib_qp/EXTRAS/SolutionAnalysis.o \
+	lib_mpc_export/acado_qpoases_interface.o \
+  lib_mpc_export/acado_integrator.o \
+  lib_mpc_export/acado_solver.o \
+  lib_mpc_export/acado_auxiliary_functions.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
 
-
-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
+lib_qp/%.o: $(PHONELIBS)/qpoases/SRC/%.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)
diff --git a/selfdrive/controls/lib/lateral_mpc/generator.cpp b/selfdrive/controls/lib/lateral_mpc/generator.cpp
index 32e2d30e3de355..9af88f9dc8a732 100644
--- a/selfdrive/controls/lib/lateral_mpc/generator.cpp
+++ b/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( );
diff --git a/selfdrive/controls/lib/lateral_mpc/mpc.c b/selfdrive/controls/lib/lateral_mpc/lateral_mpc.c
similarity index 100%
rename from selfdrive/controls/lib/lateral_mpc/mpc.c
rename to selfdrive/controls/lib/lateral_mpc/lateral_mpc.c
diff --git a/selfdrive/controls/lib/lateral_mpc/lateral_mpc.d b/selfdrive/controls/lib/lateral_mpc/lateral_mpc.d
new file mode 100644
index 00000000000000..6cac7c2e148128
--- /dev/null
+++ b/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
diff --git a/selfdrive/controls/lib/lateral_mpc/lateral_mpc.o b/selfdrive/controls/lib/lateral_mpc/lateral_mpc.o
new file mode 100644
index 00000000000000..029fda217c0111
Binary files /dev/null and b/selfdrive/controls/lib/lateral_mpc/lateral_mpc.o differ
diff --git a/selfdrive/controls/lib/lateral_mpc/mpc_export/acado_auxiliary_functions.c b/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_auxiliary_functions.c
similarity index 100%
rename from selfdrive/controls/lib/lateral_mpc/mpc_export/acado_auxiliary_functions.c
rename to selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_auxiliary_functions.c
diff --git a/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_auxiliary_functions.d b/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_auxiliary_functions.d
new file mode 100644
index 00000000000000..50c6a60d1fb846
--- /dev/null
+++ b/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_auxiliary_functions.d
@@ -0,0 +1,5 @@
+lib_mpc_export/acado_auxiliary_functions.o: \
+  lib_mpc_export/acado_auxiliary_functions.c \
+  lib_mpc_export/acado_auxiliary_functions.h \
+  lib_mpc_export/acado_common.h \
+  lib_mpc_export/acado_qpoases_interface.hpp
diff --git a/selfdrive/controls/lib/lateral_mpc/mpc_export/acado_auxiliary_functions.h b/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_auxiliary_functions.h
similarity index 100%
rename from selfdrive/controls/lib/lateral_mpc/mpc_export/acado_auxiliary_functions.h
rename to selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_auxiliary_functions.h
diff --git a/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_auxiliary_functions.o b/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_auxiliary_functions.o
new file mode 100644
index 00000000000000..07d92429b217f2
Binary files /dev/null and b/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_auxiliary_functions.o differ
diff --git a/selfdrive/controls/lib/lateral_mpc/mpc_export/acado_common.h b/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_common.h
similarity index 100%
rename from selfdrive/controls/lib/lateral_mpc/mpc_export/acado_common.h
rename to selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_common.h
diff --git a/selfdrive/controls/lib/lateral_mpc/mpc_export/acado_integrator.c b/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_integrator.c
similarity index 100%
rename from selfdrive/controls/lib/lateral_mpc/mpc_export/acado_integrator.c
rename to selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_integrator.c
diff --git a/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_integrator.d b/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_integrator.d
new file mode 100644
index 00000000000000..7c057e5ec0f3a9
--- /dev/null
+++ b/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_integrator.d
@@ -0,0 +1,3 @@
+lib_mpc_export/acado_integrator.o: lib_mpc_export/acado_integrator.c \
+  lib_mpc_export/acado_common.h \
+  lib_mpc_export/acado_qpoases_interface.hpp
diff --git a/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_integrator.o b/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_integrator.o
new file mode 100644
index 00000000000000..d08f1e41279bb7
Binary files /dev/null and b/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_integrator.o differ
diff --git a/selfdrive/controls/lib/lateral_mpc/mpc_export/acado_qpoases_interface.cpp b/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_qpoases_interface.cpp
similarity index 100%
rename from selfdrive/controls/lib/lateral_mpc/mpc_export/acado_qpoases_interface.cpp
rename to selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_qpoases_interface.cpp
diff --git a/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_qpoases_interface.d b/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_qpoases_interface.d
new file mode 100644
index 00000000000000..ba3c6ef1c831b7
--- /dev/null
+++ b/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_qpoases_interface.d
@@ -0,0 +1,24 @@
+lib_mpc_export/acado_qpoases_interface.o: \
+  lib_mpc_export/acado_qpoases_interface.cpp \
+  lib_mpc_export/acado_common.h \
+  lib_mpc_export/acado_qpoases_interface.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 \
+  ../../../../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
diff --git a/selfdrive/controls/lib/lateral_mpc/mpc_export/acado_qpoases_interface.hpp b/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_qpoases_interface.hpp
similarity index 100%
rename from selfdrive/controls/lib/lateral_mpc/mpc_export/acado_qpoases_interface.hpp
rename to selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_qpoases_interface.hpp
diff --git a/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_qpoases_interface.o b/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_qpoases_interface.o
new file mode 100644
index 00000000000000..e10c9772ab2b29
Binary files /dev/null and b/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_qpoases_interface.o differ
diff --git a/selfdrive/controls/lib/lateral_mpc/mpc_export/acado_solver.c b/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_solver.c
similarity index 100%
rename from selfdrive/controls/lib/lateral_mpc/mpc_export/acado_solver.c
rename to selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_solver.c
diff --git a/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_solver.d b/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_solver.d
new file mode 100644
index 00000000000000..2dae4b94389e48
--- /dev/null
+++ b/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_solver.d
@@ -0,0 +1,3 @@
+lib_mpc_export/acado_solver.o: lib_mpc_export/acado_solver.c \
+  lib_mpc_export/acado_common.h \
+  lib_mpc_export/acado_qpoases_interface.hpp
diff --git a/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_solver.o b/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_solver.o
new file mode 100644
index 00000000000000..f91ea40ab5e655
Binary files /dev/null and b/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_solver.o differ
diff --git a/selfdrive/controls/lib/lateral_mpc/lib_qp/Bounds.d b/selfdrive/controls/lib/lateral_mpc/lib_qp/Bounds.d
new file mode 100644
index 00000000000000..752ec5e9f3c686
--- /dev/null
+++ b/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
diff --git a/selfdrive/controls/lib/lateral_mpc/lib_qp/Bounds.o b/selfdrive/controls/lib/lateral_mpc/lib_qp/Bounds.o
new file mode 100644
index 00000000000000..ba30572669f57d
Binary files /dev/null and b/selfdrive/controls/lib/lateral_mpc/lib_qp/Bounds.o differ
diff --git a/selfdrive/controls/lib/lateral_mpc/lib_qp/Constraints.d b/selfdrive/controls/lib/lateral_mpc/lib_qp/Constraints.d
new file mode 100644
index 00000000000000..60295bc07b3dc9
--- /dev/null
+++ b/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
diff --git a/selfdrive/controls/lib/lateral_mpc/lib_qp/Constraints.o b/selfdrive/controls/lib/lateral_mpc/lib_qp/Constraints.o
new file mode 100644
index 00000000000000..d892c668667029
Binary files /dev/null and b/selfdrive/controls/lib/lateral_mpc/lib_qp/Constraints.o differ
diff --git a/selfdrive/controls/lib/lateral_mpc/lib_qp/CyclingManager.d b/selfdrive/controls/lib/lateral_mpc/lib_qp/CyclingManager.d
new file mode 100644
index 00000000000000..24145d8ed318e1
--- /dev/null
+++ b/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
diff --git a/selfdrive/controls/lib/lateral_mpc/lib_qp/CyclingManager.o b/selfdrive/controls/lib/lateral_mpc/lib_qp/CyclingManager.o
new file mode 100644
index 00000000000000..685e25d56b0c62
Binary files /dev/null and b/selfdrive/controls/lib/lateral_mpc/lib_qp/CyclingManager.o differ
diff --git a/selfdrive/controls/lib/lateral_mpc/lib_qp/EXTRAS/SolutionAnalysis.d b/selfdrive/controls/lib/lateral_mpc/lib_qp/EXTRAS/SolutionAnalysis.d
new file mode 100644
index 00000000000000..77c09376eef96e
--- /dev/null
+++ b/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
diff --git a/selfdrive/controls/lib/lateral_mpc/lib_qp/EXTRAS/SolutionAnalysis.o b/selfdrive/controls/lib/lateral_mpc/lib_qp/EXTRAS/SolutionAnalysis.o
new file mode 100644
index 00000000000000..987f1e56361641
Binary files /dev/null and b/selfdrive/controls/lib/lateral_mpc/lib_qp/EXTRAS/SolutionAnalysis.o differ
diff --git a/selfdrive/controls/lib/lateral_mpc/lib_qp/Indexlist.d b/selfdrive/controls/lib/lateral_mpc/lib_qp/Indexlist.d
new file mode 100644
index 00000000000000..6ead64cb9d8ee6
--- /dev/null
+++ b/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
diff --git a/selfdrive/controls/lib/lateral_mpc/lib_qp/Indexlist.o b/selfdrive/controls/lib/lateral_mpc/lib_qp/Indexlist.o
new file mode 100644
index 00000000000000..bd77188d453aae
Binary files /dev/null and b/selfdrive/controls/lib/lateral_mpc/lib_qp/Indexlist.o differ
diff --git a/selfdrive/controls/lib/lateral_mpc/lib_qp/MessageHandling.d b/selfdrive/controls/lib/lateral_mpc/lib_qp/MessageHandling.d
new file mode 100644
index 00000000000000..b5c6166aba1f64
--- /dev/null
+++ b/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
diff --git a/selfdrive/controls/lib/lateral_mpc/lib_qp/MessageHandling.o b/selfdrive/controls/lib/lateral_mpc/lib_qp/MessageHandling.o
new file mode 100644
index 00000000000000..87b200d0475adb
Binary files /dev/null and b/selfdrive/controls/lib/lateral_mpc/lib_qp/MessageHandling.o differ
diff --git a/selfdrive/controls/lib/lateral_mpc/lib_qp/QProblem.d b/selfdrive/controls/lib/lateral_mpc/lib_qp/QProblem.d
new file mode 100644
index 00000000000000..1f6e188af55c17
--- /dev/null
+++ b/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
diff --git a/selfdrive/controls/lib/lateral_mpc/lib_qp/QProblem.o b/selfdrive/controls/lib/lateral_mpc/lib_qp/QProblem.o
new file mode 100644
index 00000000000000..8c179beae1dc8f
Binary files /dev/null and b/selfdrive/controls/lib/lateral_mpc/lib_qp/QProblem.o differ
diff --git a/selfdrive/controls/lib/lateral_mpc/lib_qp/QProblemB.d b/selfdrive/controls/lib/lateral_mpc/lib_qp/QProblemB.d
new file mode 100644
index 00000000000000..7878a825ce5749
--- /dev/null
+++ b/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
diff --git a/selfdrive/controls/lib/lateral_mpc/lib_qp/QProblemB.o b/selfdrive/controls/lib/lateral_mpc/lib_qp/QProblemB.o
new file mode 100644
index 00000000000000..9c75cb83225246
Binary files /dev/null and b/selfdrive/controls/lib/lateral_mpc/lib_qp/QProblemB.o differ
diff --git a/selfdrive/controls/lib/lateral_mpc/lib_qp/SubjectTo.d b/selfdrive/controls/lib/lateral_mpc/lib_qp/SubjectTo.d
new file mode 100644
index 00000000000000..c896d9a9faf58d
--- /dev/null
+++ b/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
diff --git a/selfdrive/controls/lib/lateral_mpc/lib_qp/SubjectTo.o b/selfdrive/controls/lib/lateral_mpc/lib_qp/SubjectTo.o
new file mode 100644
index 00000000000000..820c930ce31ec4
Binary files /dev/null and b/selfdrive/controls/lib/lateral_mpc/lib_qp/SubjectTo.o differ
diff --git a/selfdrive/controls/lib/lateral_mpc/lib_qp/Utils.d b/selfdrive/controls/lib/lateral_mpc/lib_qp/Utils.d
new file mode 100644
index 00000000000000..58f35a065397a3
--- /dev/null
+++ b/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
diff --git a/selfdrive/controls/lib/lateral_mpc/lib_qp/Utils.o b/selfdrive/controls/lib/lateral_mpc/lib_qp/Utils.o
new file mode 100644
index 00000000000000..ef645ef188049f
Binary files /dev/null and b/selfdrive/controls/lib/lateral_mpc/lib_qp/Utils.o differ
diff --git a/selfdrive/controls/lib/lateral_mpc/libmpc.so b/selfdrive/controls/lib/lateral_mpc/libmpc.so
new file mode 100755
index 00000000000000..a0c0dc815d3b10
Binary files /dev/null and b/selfdrive/controls/lib/lateral_mpc/libmpc.so differ
diff --git a/selfdrive/controls/lib/lateral_mpc/libmpc_py.py b/selfdrive/controls/lib/lateral_mpc/libmpc_py.py
index 5970dab9039709..1537d2f6c027dd 100644
--- a/selfdrive/controls/lib/lateral_mpc/libmpc_py.py
+++ b/selfdrive/controls/lib/lateral_mpc/libmpc_py.py
@@ -4,7 +4,7 @@
 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()
diff --git a/selfdrive/controls/lib/longitudinal_mpc/Makefile b/selfdrive/controls/lib/longitudinal_mpc/Makefile
index 409715cf17e9d6..77718adfb4df9f 100644
--- a/selfdrive/controls/lib/longitudinal_mpc/Makefile
+++ b/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 \
-  qp/Constraints.o \
-  qp/CyclingManager.o \
-  qp/Indexlist.o \
-  qp/MessageHandling.o \
-  qp/QProblem.o \
-  qp/QProblemB.o \
-  qp/SubjectTo.o \
-  qp/Utils.o \
-  qp/EXTRAS/SolutionAnalysis.o \
-	mpc_export/acado_qpoases_interface.o \
-  mpc_export/acado_integrator.o \
-  mpc_export/acado_solver.o \
-  mpc_export/acado_auxiliary_functions.o \
-  mpc.o
+  lib_qp/Bounds.o \
+  lib_qp/Constraints.o \
+  lib_qp/CyclingManager.o \
+  lib_qp/Indexlist.o \
+  lib_qp/MessageHandling.o \
+  lib_qp/QProblem.o \
+  lib_qp/QProblemB.o \
+  lib_qp/SubjectTo.o \
+  lib_qp/Utils.o \
+  lib_qp/EXTRAS/SolutionAnalysis.o \
+	lib_mpc_export/acado_qpoases_interface.o \
+  lib_mpc_export/acado_integrator.o \
+  lib_mpc_export/acado_solver.o \
+  lib_mpc_export/acado_auxiliary_functions.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
-	cp libcommampc1.so libcommampc2.so
+libmpc2.so: libmpc1.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/ \
-           $(QPOASES_FLAGS) \
-           -c -o '$@' '$<'
-
-qp/EXTRAS/%.o: $(PHONELIBS)/qpoases/SRC/EXTRAS/%.cpp
-	@echo "[ CXX ] $@"
-	mkdir -p 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 '$@' '$<'
 
@@ -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)
diff --git a/selfdrive/controls/lib/longitudinal_mpc/generator.cpp b/selfdrive/controls/lib/longitudinal_mpc/generator.cpp
index 30b0bd858a82d0..f5c95394d8d3f1 100644
--- a/selfdrive/controls/lib/longitudinal_mpc/generator.cpp
+++ b/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( );
diff --git a/selfdrive/controls/lib/longitudinal_mpc/mpc_export/acado_auxiliary_functions.c b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_auxiliary_functions.c
similarity index 100%
rename from selfdrive/controls/lib/longitudinal_mpc/mpc_export/acado_auxiliary_functions.c
rename to selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_auxiliary_functions.c
diff --git a/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_auxiliary_functions.d b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_auxiliary_functions.d
new file mode 100644
index 00000000000000..50c6a60d1fb846
--- /dev/null
+++ b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_auxiliary_functions.d
@@ -0,0 +1,5 @@
+lib_mpc_export/acado_auxiliary_functions.o: \
+  lib_mpc_export/acado_auxiliary_functions.c \
+  lib_mpc_export/acado_auxiliary_functions.h \
+  lib_mpc_export/acado_common.h \
+  lib_mpc_export/acado_qpoases_interface.hpp
diff --git a/selfdrive/controls/lib/longitudinal_mpc/mpc_export/acado_auxiliary_functions.h b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_auxiliary_functions.h
similarity index 100%
rename from selfdrive/controls/lib/longitudinal_mpc/mpc_export/acado_auxiliary_functions.h
rename to selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_auxiliary_functions.h
diff --git a/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_auxiliary_functions.o b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_auxiliary_functions.o
new file mode 100644
index 00000000000000..65182c5bb1ee55
Binary files /dev/null and b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_auxiliary_functions.o differ
diff --git a/selfdrive/controls/lib/longitudinal_mpc/mpc_export/acado_common.h b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_common.h
similarity index 96%
rename from selfdrive/controls/lib/longitudinal_mpc/mpc_export/acado_common.h
rename to selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_common.h
index 1f1c21e6e017fd..cecba6e7408cba 100644
--- a/selfdrive/controls/lib/longitudinal_mpc/mpc_export/acado_common.h
+++ b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_common.h
@@ -64,7 +64,7 @@ extern "C"
 /** Number of control/estimation intervals. */
 #define ACADO_N 20
 /** Number of online data values. */
-#define ACADO_NOD 1
+#define ACADO_NOD 2
 /** Number of path constraints. */
 #define ACADO_NPAC 0
 /** Number of control variables. */
@@ -114,11 +114,11 @@ real_t x[ 126 ];
  */
 real_t u[ 20 ];
 
-/** Column vector of size: 21
+/** Matrix of size: 21 x 2 (row major format)
  * 
  *  Matrix containing 21 online data vectors.
  */
-real_t od[ 21 ];
+real_t od[ 42 ];
 
 /** Column vector of size: 80
  * 
@@ -155,16 +155,19 @@ real_t x0[ 6 ];
  */
 typedef struct ACADOworkspace_
 {
+/** Column vector of size: 10 */
+real_t rhs_aux[ 10 ];
+
 real_t rk_ttt;
 
-/** Row vector of size: 50 */
-real_t rk_xxx[ 50 ];
+/** Row vector of size: 51 */
+real_t rk_xxx[ 51 ];
 
 /** Matrix of size: 4 x 48 (row major format) */
 real_t rk_kkk[ 192 ];
 
-/** Row vector of size: 50 */
-real_t state[ 50 ];
+/** Row vector of size: 51 */
+real_t state[ 51 ];
 
 /** Column vector of size: 120 */
 real_t d[ 120 ];
@@ -184,8 +187,8 @@ real_t evGu[ 120 ];
 /** Column vector of size: 30 */
 real_t objAuxVar[ 30 ];
 
-/** Row vector of size: 8 */
-real_t objValueIn[ 8 ];
+/** Row vector of size: 9 */
+real_t objValueIn[ 9 ];
 
 /** Row vector of size: 32 */
 real_t objValueOut[ 32 ];
diff --git a/selfdrive/controls/lib/longitudinal_mpc/mpc_export/acado_integrator.c b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_integrator.c
similarity index 94%
rename from selfdrive/controls/lib/longitudinal_mpc/mpc_export/acado_integrator.c
rename to selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_integrator.c
index 584d8fb857f530..2e77d54147dbee 100644
--- a/selfdrive/controls/lib/longitudinal_mpc/mpc_export/acado_integrator.c
+++ b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_integrator.c
@@ -25,14 +25,28 @@ void acado_rhs_forw(const real_t* in, real_t* out)
 const real_t* xd = in;
 const real_t* u = in + 48;
 const real_t* od = in + 49;
+/* Vector of auxiliary variables; number of elements: 10. */
+real_t* a = acadoWorkspace.rhs_aux;
+
+/* Compute intermediate quantities: */
+a[0] = (exp((((((real_t)(0.0000000000000000e+00)-od[0])*xd[5])*xd[5])/(real_t)(2.0000000000000000e+00))));
+a[1] = ((real_t)(1.0000000000000000e+00)/(real_t)(2.0000000000000000e+00));
+a[2] = (exp((((((real_t)(0.0000000000000000e+00)-od[0])*xd[5])*xd[5])/(real_t)(2.0000000000000000e+00))));
+a[3] = (((((((real_t)(0.0000000000000000e+00)-od[0])*xd[36])*xd[5])+((((real_t)(0.0000000000000000e+00)-od[0])*xd[5])*xd[36]))*a[1])*a[2]);
+a[4] = (((((((real_t)(0.0000000000000000e+00)-od[0])*xd[37])*xd[5])+((((real_t)(0.0000000000000000e+00)-od[0])*xd[5])*xd[37]))*a[1])*a[2]);
+a[5] = (((((((real_t)(0.0000000000000000e+00)-od[0])*xd[38])*xd[5])+((((real_t)(0.0000000000000000e+00)-od[0])*xd[5])*xd[38]))*a[1])*a[2]);
+a[6] = (((((((real_t)(0.0000000000000000e+00)-od[0])*xd[39])*xd[5])+((((real_t)(0.0000000000000000e+00)-od[0])*xd[5])*xd[39]))*a[1])*a[2]);
+a[7] = (((((((real_t)(0.0000000000000000e+00)-od[0])*xd[40])*xd[5])+((((real_t)(0.0000000000000000e+00)-od[0])*xd[5])*xd[40]))*a[1])*a[2]);
+a[8] = (((((((real_t)(0.0000000000000000e+00)-od[0])*xd[41])*xd[5])+((((real_t)(0.0000000000000000e+00)-od[0])*xd[5])*xd[41]))*a[1])*a[2]);
+a[9] = (((((((real_t)(0.0000000000000000e+00)-od[0])*xd[47])*xd[5])+((((real_t)(0.0000000000000000e+00)-od[0])*xd[5])*xd[47]))*a[1])*a[2]);
 
 /* Compute outputs: */
 out[0] = xd[1];
 out[1] = xd[2];
 out[2] = u[0];
 out[3] = xd[4];
-out[4] = xd[5];
-out[5] = (((real_t)(0.0000000000000000e+00)-od[0])*xd[5]);
+out[4] = (od[1]*a[0]);
+out[5] = (real_t)(1.0000000000000000e+00);
 out[6] = xd[12];
 out[7] = xd[13];
 out[8] = xd[14];
@@ -57,24 +71,24 @@ out[26] = xd[32];
 out[27] = xd[33];
 out[28] = xd[34];
 out[29] = xd[35];
-out[30] = xd[36];
-out[31] = xd[37];
-out[32] = xd[38];
-out[33] = xd[39];
-out[34] = xd[40];
-out[35] = xd[41];
-out[36] = (((real_t)(0.0000000000000000e+00)-od[0])*xd[36]);
-out[37] = (((real_t)(0.0000000000000000e+00)-od[0])*xd[37]);
-out[38] = (((real_t)(0.0000000000000000e+00)-od[0])*xd[38]);
-out[39] = (((real_t)(0.0000000000000000e+00)-od[0])*xd[39]);
-out[40] = (((real_t)(0.0000000000000000e+00)-od[0])*xd[40]);
-out[41] = (((real_t)(0.0000000000000000e+00)-od[0])*xd[41]);
+out[30] = (od[1]*a[3]);
+out[31] = (od[1]*a[4]);
+out[32] = (od[1]*a[5]);
+out[33] = (od[1]*a[6]);
+out[34] = (od[1]*a[7]);
+out[35] = (od[1]*a[8]);
+out[36] = (real_t)(0.0000000000000000e+00);
+out[37] = (real_t)(0.0000000000000000e+00);
+out[38] = (real_t)(0.0000000000000000e+00);
+out[39] = (real_t)(0.0000000000000000e+00);
+out[40] = (real_t)(0.0000000000000000e+00);
+out[41] = (real_t)(0.0000000000000000e+00);
 out[42] = xd[43];
 out[43] = xd[44];
 out[44] = (real_t)(1.0000000000000000e+00);
 out[45] = xd[46];
-out[46] = xd[47];
-out[47] = (((real_t)(0.0000000000000000e+00)-od[0])*xd[47]);
+out[46] = (od[1]*a[9]);
+out[47] = (real_t)(0.0000000000000000e+00);
 }
 
 /* Fixed step size:0.2 */
@@ -130,6 +144,7 @@ rk_eta[46] = 0.0000000000000000e+00;
 rk_eta[47] = 0.0000000000000000e+00;
 acadoWorkspace.rk_xxx[48] = rk_eta[48];
 acadoWorkspace.rk_xxx[49] = rk_eta[49];
+acadoWorkspace.rk_xxx[50] = rk_eta[50];
 
 for (run1 = 0; run1 < 1; ++run1)
 {
diff --git a/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_integrator.d b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_integrator.d
new file mode 100644
index 00000000000000..7c057e5ec0f3a9
--- /dev/null
+++ b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_integrator.d
@@ -0,0 +1,3 @@
+lib_mpc_export/acado_integrator.o: lib_mpc_export/acado_integrator.c \
+  lib_mpc_export/acado_common.h \
+  lib_mpc_export/acado_qpoases_interface.hpp
diff --git a/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_integrator.o b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_integrator.o
new file mode 100644
index 00000000000000..0008b139779a03
Binary files /dev/null and b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_integrator.o differ
diff --git a/selfdrive/controls/lib/longitudinal_mpc/mpc_export/acado_qpoases_interface.cpp b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_qpoases_interface.cpp
similarity index 100%
rename from selfdrive/controls/lib/longitudinal_mpc/mpc_export/acado_qpoases_interface.cpp
rename to selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_qpoases_interface.cpp
diff --git a/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_qpoases_interface.d b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_qpoases_interface.d
new file mode 100644
index 00000000000000..ba3c6ef1c831b7
--- /dev/null
+++ b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_qpoases_interface.d
@@ -0,0 +1,24 @@
+lib_mpc_export/acado_qpoases_interface.o: \
+  lib_mpc_export/acado_qpoases_interface.cpp \
+  lib_mpc_export/acado_common.h \
+  lib_mpc_export/acado_qpoases_interface.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 \
+  ../../../../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
diff --git a/selfdrive/controls/lib/longitudinal_mpc/mpc_export/acado_qpoases_interface.hpp b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_qpoases_interface.hpp
similarity index 100%
rename from selfdrive/controls/lib/longitudinal_mpc/mpc_export/acado_qpoases_interface.hpp
rename to selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_qpoases_interface.hpp
diff --git a/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_qpoases_interface.o b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_qpoases_interface.o
new file mode 100644
index 00000000000000..ce919eb77f5fa8
Binary files /dev/null and b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_qpoases_interface.o differ
diff --git a/selfdrive/controls/lib/longitudinal_mpc/mpc_export/acado_solver.c b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_solver.c
similarity index 99%
rename from selfdrive/controls/lib/longitudinal_mpc/mpc_export/acado_solver.c
rename to selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_solver.c
index a06e4a38d6dc90..676787bbfcbc7b 100644
--- a/selfdrive/controls/lib/longitudinal_mpc/mpc_export/acado_solver.c
+++ b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_solver.c
@@ -45,7 +45,8 @@ acadoWorkspace.state[4] = acadoVariables.x[lRun1 * 6 + 4];
 acadoWorkspace.state[5] = acadoVariables.x[lRun1 * 6 + 5];
 
 acadoWorkspace.state[48] = acadoVariables.u[lRun1];
-acadoWorkspace.state[49] = acadoVariables.od[lRun1];
+acadoWorkspace.state[49] = acadoVariables.od[lRun1 * 2];
+acadoWorkspace.state[50] = acadoVariables.od[lRun1 * 2 + 1];
 
 ret = acado_integrate(acadoWorkspace.state, 1, lRun1);
 
@@ -382,7 +383,8 @@ acadoWorkspace.objValueIn[3] = acadoVariables.x[runObj * 6 + 3];
 acadoWorkspace.objValueIn[4] = acadoVariables.x[runObj * 6 + 4];
 acadoWorkspace.objValueIn[5] = acadoVariables.x[runObj * 6 + 5];
 acadoWorkspace.objValueIn[6] = acadoVariables.u[runObj];
-acadoWorkspace.objValueIn[7] = acadoVariables.od[runObj];
+acadoWorkspace.objValueIn[7] = acadoVariables.od[runObj * 2];
+acadoWorkspace.objValueIn[8] = acadoVariables.od[runObj * 2 + 1];
 
 acado_evaluateLSQ( acadoWorkspace.objValueIn, acadoWorkspace.objValueOut );
 acadoWorkspace.Dy[runObj * 4] = acadoWorkspace.objValueOut[0];
@@ -401,7 +403,8 @@ acadoWorkspace.objValueIn[2] = acadoVariables.x[122];
 acadoWorkspace.objValueIn[3] = acadoVariables.x[123];
 acadoWorkspace.objValueIn[4] = acadoVariables.x[124];
 acadoWorkspace.objValueIn[5] = acadoVariables.x[125];
-acadoWorkspace.objValueIn[6] = acadoVariables.od[20];
+acadoWorkspace.objValueIn[6] = acadoVariables.od[40];
+acadoWorkspace.objValueIn[7] = acadoVariables.od[41];
 acado_evaluateLSQEndTerm( acadoWorkspace.objValueIn, acadoWorkspace.objValueOut );
 
 acadoWorkspace.DyN[0] = acadoWorkspace.objValueOut[0];
@@ -5202,7 +5205,8 @@ acadoWorkspace.state[3] = acadoVariables.x[index * 6 + 3];
 acadoWorkspace.state[4] = acadoVariables.x[index * 6 + 4];
 acadoWorkspace.state[5] = acadoVariables.x[index * 6 + 5];
 acadoWorkspace.state[48] = acadoVariables.u[index];
-acadoWorkspace.state[49] = acadoVariables.od[index];
+acadoWorkspace.state[49] = acadoVariables.od[index * 2];
+acadoWorkspace.state[50] = acadoVariables.od[index * 2 + 1];
 
 acado_integrate(acadoWorkspace.state, index == 0, index);
 
@@ -5253,7 +5257,8 @@ else
 {
 acadoWorkspace.state[48] = acadoVariables.u[19];
 }
-acadoWorkspace.state[49] = acadoVariables.od[20];
+acadoWorkspace.state[49] = acadoVariables.od[40];
+acadoWorkspace.state[50] = acadoVariables.od[41];
 
 acado_integrate(acadoWorkspace.state, 1, 19);
 
@@ -5328,7 +5333,8 @@ acadoWorkspace.objValueIn[3] = acadoVariables.x[lRun1 * 6 + 3];
 acadoWorkspace.objValueIn[4] = acadoVariables.x[lRun1 * 6 + 4];
 acadoWorkspace.objValueIn[5] = acadoVariables.x[lRun1 * 6 + 5];
 acadoWorkspace.objValueIn[6] = acadoVariables.u[lRun1];
-acadoWorkspace.objValueIn[7] = acadoVariables.od[lRun1];
+acadoWorkspace.objValueIn[7] = acadoVariables.od[lRun1 * 2];
+acadoWorkspace.objValueIn[8] = acadoVariables.od[lRun1 * 2 + 1];
 
 acado_evaluateLSQ( acadoWorkspace.objValueIn, acadoWorkspace.objValueOut );
 acadoWorkspace.Dy[lRun1 * 4] = acadoWorkspace.objValueOut[0] - acadoVariables.y[lRun1 * 4];
@@ -5342,7 +5348,8 @@ acadoWorkspace.objValueIn[2] = acadoVariables.x[122];
 acadoWorkspace.objValueIn[3] = acadoVariables.x[123];
 acadoWorkspace.objValueIn[4] = acadoVariables.x[124];
 acadoWorkspace.objValueIn[5] = acadoVariables.x[125];
-acadoWorkspace.objValueIn[6] = acadoVariables.od[20];
+acadoWorkspace.objValueIn[6] = acadoVariables.od[40];
+acadoWorkspace.objValueIn[7] = acadoVariables.od[41];
 acado_evaluateLSQEndTerm( acadoWorkspace.objValueIn, acadoWorkspace.objValueOut );
 acadoWorkspace.DyN[0] = acadoWorkspace.objValueOut[0] - acadoVariables.yN[0];
 acadoWorkspace.DyN[1] = acadoWorkspace.objValueOut[1] - acadoVariables.yN[1];
diff --git a/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_solver.d b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_solver.d
new file mode 100644
index 00000000000000..2dae4b94389e48
--- /dev/null
+++ b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_solver.d
@@ -0,0 +1,3 @@
+lib_mpc_export/acado_solver.o: lib_mpc_export/acado_solver.c \
+  lib_mpc_export/acado_common.h \
+  lib_mpc_export/acado_qpoases_interface.hpp
diff --git a/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_solver.o b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_solver.o
new file mode 100644
index 00000000000000..8f2fcfbdf156ea
Binary files /dev/null and b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_solver.o differ
diff --git a/selfdrive/controls/lib/longitudinal_mpc/lib_qp/Bounds.d b/selfdrive/controls/lib/longitudinal_mpc/lib_qp/Bounds.d
new file mode 100644
index 00000000000000..752ec5e9f3c686
--- /dev/null
+++ b/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
diff --git a/selfdrive/controls/lib/longitudinal_mpc/lib_qp/Bounds.o b/selfdrive/controls/lib/longitudinal_mpc/lib_qp/Bounds.o
new file mode 100644
index 00000000000000..b770e3240eec05
Binary files /dev/null and b/selfdrive/controls/lib/longitudinal_mpc/lib_qp/Bounds.o differ
diff --git a/selfdrive/controls/lib/longitudinal_mpc/lib_qp/Constraints.d b/selfdrive/controls/lib/longitudinal_mpc/lib_qp/Constraints.d
new file mode 100644
index 00000000000000..60295bc07b3dc9
--- /dev/null
+++ b/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
diff --git a/selfdrive/controls/lib/longitudinal_mpc/lib_qp/Constraints.o b/selfdrive/controls/lib/longitudinal_mpc/lib_qp/Constraints.o
new file mode 100644
index 00000000000000..4d47fe901918b4
Binary files /dev/null and b/selfdrive/controls/lib/longitudinal_mpc/lib_qp/Constraints.o differ
diff --git a/selfdrive/controls/lib/longitudinal_mpc/lib_qp/CyclingManager.d b/selfdrive/controls/lib/longitudinal_mpc/lib_qp/CyclingManager.d
new file mode 100644
index 00000000000000..24145d8ed318e1
--- /dev/null
+++ b/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
diff --git a/selfdrive/controls/lib/longitudinal_mpc/lib_qp/CyclingManager.o b/selfdrive/controls/lib/longitudinal_mpc/lib_qp/CyclingManager.o
new file mode 100644
index 00000000000000..ec3e83d112b8c5
Binary files /dev/null and b/selfdrive/controls/lib/longitudinal_mpc/lib_qp/CyclingManager.o differ
diff --git a/selfdrive/controls/lib/longitudinal_mpc/lib_qp/EXTRAS/SolutionAnalysis.d b/selfdrive/controls/lib/longitudinal_mpc/lib_qp/EXTRAS/SolutionAnalysis.d
new file mode 100644
index 00000000000000..77c09376eef96e
--- /dev/null
+++ b/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
diff --git a/selfdrive/controls/lib/longitudinal_mpc/lib_qp/EXTRAS/SolutionAnalysis.o b/selfdrive/controls/lib/longitudinal_mpc/lib_qp/EXTRAS/SolutionAnalysis.o
new file mode 100644
index 00000000000000..71cbd4ce3aa5af
Binary files /dev/null and b/selfdrive/controls/lib/longitudinal_mpc/lib_qp/EXTRAS/SolutionAnalysis.o differ
diff --git a/selfdrive/controls/lib/longitudinal_mpc/lib_qp/Indexlist.d b/selfdrive/controls/lib/longitudinal_mpc/lib_qp/Indexlist.d
new file mode 100644
index 00000000000000..6ead64cb9d8ee6
--- /dev/null
+++ b/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
diff --git a/selfdrive/controls/lib/longitudinal_mpc/lib_qp/Indexlist.o b/selfdrive/controls/lib/longitudinal_mpc/lib_qp/Indexlist.o
new file mode 100644
index 00000000000000..9ef68d5bfa7d38
Binary files /dev/null and b/selfdrive/controls/lib/longitudinal_mpc/lib_qp/Indexlist.o differ
diff --git a/selfdrive/controls/lib/longitudinal_mpc/lib_qp/MessageHandling.d b/selfdrive/controls/lib/longitudinal_mpc/lib_qp/MessageHandling.d
new file mode 100644
index 00000000000000..b5c6166aba1f64
--- /dev/null
+++ b/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
diff --git a/selfdrive/controls/lib/longitudinal_mpc/lib_qp/MessageHandling.o b/selfdrive/controls/lib/longitudinal_mpc/lib_qp/MessageHandling.o
new file mode 100644
index 00000000000000..87b200d0475adb
Binary files /dev/null and b/selfdrive/controls/lib/longitudinal_mpc/lib_qp/MessageHandling.o differ
diff --git a/selfdrive/controls/lib/longitudinal_mpc/lib_qp/QProblem.d b/selfdrive/controls/lib/longitudinal_mpc/lib_qp/QProblem.d
new file mode 100644
index 00000000000000..1f6e188af55c17
--- /dev/null
+++ b/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
diff --git a/selfdrive/controls/lib/longitudinal_mpc/lib_qp/QProblem.o b/selfdrive/controls/lib/longitudinal_mpc/lib_qp/QProblem.o
new file mode 100644
index 00000000000000..81a50555553aa0
Binary files /dev/null and b/selfdrive/controls/lib/longitudinal_mpc/lib_qp/QProblem.o differ
diff --git a/selfdrive/controls/lib/longitudinal_mpc/lib_qp/QProblemB.d b/selfdrive/controls/lib/longitudinal_mpc/lib_qp/QProblemB.d
new file mode 100644
index 00000000000000..7878a825ce5749
--- /dev/null
+++ b/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
diff --git a/selfdrive/controls/lib/longitudinal_mpc/lib_qp/QProblemB.o b/selfdrive/controls/lib/longitudinal_mpc/lib_qp/QProblemB.o
new file mode 100644
index 00000000000000..a850439245f58e
Binary files /dev/null and b/selfdrive/controls/lib/longitudinal_mpc/lib_qp/QProblemB.o differ
diff --git a/selfdrive/controls/lib/longitudinal_mpc/lib_qp/SubjectTo.d b/selfdrive/controls/lib/longitudinal_mpc/lib_qp/SubjectTo.d
new file mode 100644
index 00000000000000..c896d9a9faf58d
--- /dev/null
+++ b/selfdrive/controls/lib/longitudinal_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
diff --git a/selfdrive/controls/lib/longitudinal_mpc/lib_qp/SubjectTo.o b/selfdrive/controls/lib/longitudinal_mpc/lib_qp/SubjectTo.o
new file mode 100644
index 00000000000000..827237a682079f
Binary files /dev/null and b/selfdrive/controls/lib/longitudinal_mpc/lib_qp/SubjectTo.o differ
diff --git a/selfdrive/controls/lib/longitudinal_mpc/lib_qp/Utils.d b/selfdrive/controls/lib/longitudinal_mpc/lib_qp/Utils.d
new file mode 100644
index 00000000000000..58f35a065397a3
--- /dev/null
+++ b/selfdrive/controls/lib/longitudinal_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
diff --git a/selfdrive/controls/lib/longitudinal_mpc/lib_qp/Utils.o b/selfdrive/controls/lib/longitudinal_mpc/lib_qp/Utils.o
new file mode 100644
index 00000000000000..ef645ef188049f
Binary files /dev/null and b/selfdrive/controls/lib/longitudinal_mpc/lib_qp/Utils.o differ
diff --git a/selfdrive/controls/lib/longitudinal_mpc/libmpc1.so b/selfdrive/controls/lib/longitudinal_mpc/libmpc1.so
new file mode 100755
index 00000000000000..232e9a158b206c
Binary files /dev/null and b/selfdrive/controls/lib/longitudinal_mpc/libmpc1.so differ
diff --git a/selfdrive/controls/lib/longitudinal_mpc/libmpc_py.py b/selfdrive/controls/lib/longitudinal_mpc/libmpc_py.py
index b45021a97f0d7a..658ee1b85bbcfb 100644
--- a/selfdrive/controls/lib/longitudinal_mpc/libmpc_py.py
+++ b/selfdrive/controls/lib/longitudinal_mpc/libmpc_py.py
@@ -8,7 +8,7 @@
 
 
 def _get_libmpc(mpc_id):
-    libmpc_fn = os.path.join(mpc_dir, "libcommampc%d.so" % mpc_id)
+    libmpc_fn = os.path.join(mpc_dir, "libmpc%d.so" % mpc_id)
 
     ffi = FFI()
     ffi.cdef("""
@@ -24,14 +24,14 @@ def _get_libmpc(mpc_id):
     double j_ego[21];
     double x_l[21];
     double v_l[21];
-    double a_l[21];
+    double t[21];
     double cost;
     } log_t;
 
     void init(double ttcCost, double distanceCost, double accelerationCost, double jerkCost);
     void init_with_simulation(double v_ego, double x_l, double v_l, double a_l, double l);
     int run_mpc(state_t * x0, log_t * solution,
-                double l);
+                double l, double a_l_0);
     """)
 
     return (ffi, ffi.dlopen(libmpc_fn))
diff --git a/selfdrive/controls/lib/longitudinal_mpc/mpc.c b/selfdrive/controls/lib/longitudinal_mpc/longitudinal_mpc.c
similarity index 84%
rename from selfdrive/controls/lib/longitudinal_mpc/mpc.c
rename to selfdrive/controls/lib/longitudinal_mpc/longitudinal_mpc.c
index c9226d67e8c9c1..c0b43e53c93771 100644
--- a/selfdrive/controls/lib/longitudinal_mpc/mpc.c
+++ b/selfdrive/controls/lib/longitudinal_mpc/longitudinal_mpc.c
@@ -2,6 +2,7 @@
 #include "acado_auxiliary_functions.h"
 
 #include <stdio.h>
+#include <math.h>
 
 #define NX          ACADO_NX  /* Number of differential state variables.  */
 #define NXA         ACADO_NXA /* Number of algebraic variables. */
@@ -28,7 +29,7 @@ typedef struct {
   double j_ego[N+1];
 	double x_l[N+1];
 	double v_l[N+1];
-	double a_l[N+1];
+	double t[N+1];
 	double cost;
 } log_t;
 
@@ -65,34 +66,47 @@ void init(double ttcCost, double distanceCost, double accelerationCost, double j
 
 }
 
-void init_with_simulation(double v_ego, double x_l, double v_l, double a_l, double l){
+void init_with_simulation(double v_ego, double x_l_0, double v_l_0, double a_l_0, double l){
   int i;
   double x_ego = 0.0;
   double a_ego = 0.0;
 
+  double x_l = x_l_0;
+  double v_l = v_l_0;
+  double a_l = a_l_0;
+
+
   if (v_ego > v_l){
     a_ego = -(v_ego - v_l) * (v_ego - v_l) / (2.0 * x_l + 0.01) + a_l;
   }
   double dt = 0.2;
+  double t = 0.;
 
   for (i = 0; i < N + 1; ++i){
     if (i > 4){
       dt = 0.6;
     }
+
+    // Directly calculate a_l to prevent instabilites due to discretization
+    a_l = a_l_0 * exp(-l * t * t / 2);
+
+    /* printf("x_e: %.2f\t v_e: %.2f\t a_e: %.2f\t", x_ego, v_ego, a_ego); */
+    /* printf("x_l: %.2f\t v_l: %.2f\t a_l: %.2f\n", x_l, v_l, a_l); */
+
     acadoVariables.x[i*NX] = x_ego;
     acadoVariables.x[i*NX+1] = v_ego;
     acadoVariables.x[i*NX+2] = a_ego;
 
     acadoVariables.x[i*NX+3] = x_l;
     acadoVariables.x[i*NX+4] = v_l;
-    acadoVariables.x[i*NX+5] = a_l;
+    acadoVariables.x[i*NX+5] = t;
 
     x_ego += v_ego * dt;
     v_ego += a_ego * dt;
 
     x_l += v_l * dt;
     v_l += a_l * dt;
-    a_l += -l * a_l * dt;
+    t += dt;
 
     if (v_ego <= 0.0) {
       v_ego = 0.0;
@@ -104,11 +118,12 @@ void init_with_simulation(double v_ego, double x_l, double v_l, double a_l, doub
   for (i = 0; i < NYN; ++i)  acadoVariables.yN[ i ] = 0.0;
 }
 
-int run_mpc(state_t * x0, log_t * solution, double l){
+int run_mpc(state_t * x0, log_t * solution, double l, double a_l_0){
   int i;
 
   for (i = 0; i <= NOD * N; i+= NOD){
     acadoVariables.od[i] = l;
+    acadoVariables.od[i+1] = a_l_0;
   }
 
   acadoVariables.x[0] = acadoVariables.x0[0] = x0->x_ego;
@@ -116,7 +131,7 @@ int run_mpc(state_t * x0, log_t * solution, double l){
   acadoVariables.x[2] = acadoVariables.x0[2] = x0->a_ego;
   acadoVariables.x[3] = acadoVariables.x0[3] = x0->x_l;
   acadoVariables.x[4] = acadoVariables.x0[4] = x0->v_l;
-  acadoVariables.x[5] = acadoVariables.x0[5] = x0->a_l;
+  acadoVariables.x[5] = acadoVariables.x0[5] = 0.0;
 
   acado_preparationStep();
   acado_feedbackStep();
@@ -127,7 +142,7 @@ int run_mpc(state_t * x0, log_t * solution, double l){
     solution->a_ego[i] = acadoVariables.x[i*NX+2];
 		solution->x_l[i] = acadoVariables.x[i*NX+3];
 		solution->v_l[i] = acadoVariables.x[i*NX+4];
-		solution->a_l[i] = acadoVariables.x[i*NX+5];
+		solution->t[i] = acadoVariables.x[i*NX+5];
 
     solution->j_ego[i] = acadoVariables.u[i];
 	}
diff --git a/selfdrive/controls/lib/longitudinal_mpc/longitudinal_mpc.d b/selfdrive/controls/lib/longitudinal_mpc/longitudinal_mpc.d
new file mode 100644
index 00000000000000..34a2a0d20ce304
--- /dev/null
+++ b/selfdrive/controls/lib/longitudinal_mpc/longitudinal_mpc.d
@@ -0,0 +1,3 @@
+longitudinal_mpc.o: longitudinal_mpc.c lib_mpc_export/acado_common.h \
+  lib_mpc_export/acado_qpoases_interface.hpp \
+  lib_mpc_export/acado_auxiliary_functions.h
diff --git a/selfdrive/controls/lib/longitudinal_mpc/longitudinal_mpc.o b/selfdrive/controls/lib/longitudinal_mpc/longitudinal_mpc.o
new file mode 100644
index 00000000000000..e899d7b785a16f
Binary files /dev/null and b/selfdrive/controls/lib/longitudinal_mpc/longitudinal_mpc.o differ
diff --git a/selfdrive/controls/lib/planner.py b/selfdrive/controls/lib/planner.py
index 32d427f84f14eb..e2defeeb937c46 100755
--- a/selfdrive/controls/lib/planner.py
+++ b/selfdrive/controls/lib/planner.py
@@ -161,7 +161,6 @@ def send_mpc_solution(self, qp_iterations, calculation_time):
     dat.liveLongitudinalMpc.aEgo = list(self.mpc_solution[0].a_ego)
     dat.liveLongitudinalMpc.xLead = list(self.mpc_solution[0].x_l)
     dat.liveLongitudinalMpc.vLead = list(self.mpc_solution[0].v_l)
-    dat.liveLongitudinalMpc.aLead = list(self.mpc_solution[0].a_l)
     dat.liveLongitudinalMpc.cost = self.mpc_solution[0].cost
     dat.liveLongitudinalMpc.aLeadTau = self.a_lead_tau
     dat.liveLongitudinalMpc.qpIterations = qp_iterations
@@ -193,11 +192,12 @@ def update(self, CS, lead, v_cruise_setpoint):
       v_lead = max(0.0, lead.vLead)
       a_lead = lead.aLeadK
 
+
       if (v_lead < 0.1 or -a_lead / 2.0 > v_lead):
         v_lead = 0.0
         a_lead = 0.0
 
-      self.a_lead_tau = max(lead.aLeadTau, -a_lead / (v_lead + 0.01))
+      self.a_lead_tau = max(lead.aLeadTau, (a_lead**2 * math.pi) / (2 * (v_lead + 0.01)**2))
       self.new_lead = False
       if not self.prev_lead_status or abs(x_lead - self.prev_lead_x) > 2.5:
         self.libmpc.init_with_simulation(self.v_mpc, x_lead, v_lead, a_lead, self.a_lead_tau)
@@ -207,18 +207,17 @@ def update(self, CS, lead, v_cruise_setpoint):
       self.prev_lead_x = x_lead
       self.cur_state[0].x_l = x_lead
       self.cur_state[0].v_l = v_lead
-      self.cur_state[0].a_l = a_lead
     else:
       self.prev_lead_status = False
       # Fake a fast lead car, so mpc keeps running
       self.cur_state[0].x_l = 50.0
       self.cur_state[0].v_l = CS.vEgo + 10.0
-      self.cur_state[0].a_l = 0.0
+      a_lead = 0.0
       self.a_lead_tau = _LEAD_ACCEL_TAU
 
     # Calculate mpc
     t = sec_since_boot()
-    n_its = self.libmpc.run_mpc(self.cur_state, self.mpc_solution, self.a_lead_tau)
+    n_its = self.libmpc.run_mpc(self.cur_state, self.mpc_solution, self.a_lead_tau, a_lead)
     duration = int((sec_since_boot() - t) * 1e9)
     self.send_mpc_solution(n_its, duration)
 
diff --git a/selfdrive/locationd/calibrationd.py b/selfdrive/locationd/calibrationd.py
index fcb78e5fed74f4..523302f11a9365 100755
--- a/selfdrive/locationd/calibrationd.py
+++ b/selfdrive/locationd/calibrationd.py
@@ -26,7 +26,8 @@
 VP_RATE_LIM = 2. * DT    # 2 px/s
 VP_INIT = np.array([W/2., H/2.])
 EXTERNAL_PATH = os.path.dirname(os.path.abspath(__file__))
-VP_VALIDITY_CORNERS = np.array([[-200., -200.], [200., 200.]])  + VP_INIT
+# big model is 864x288
+VP_VALIDITY_CORNERS = np.array([[-150., -200.], [150., 200.]])  + VP_INIT
 GRID_WEIGHT_INIT = 2e6
 MAX_LINES = 500    # max lines to avoid over computation
 
diff --git a/selfdrive/locationd/get_vp.c b/selfdrive/locationd/get_vp.c
index c9ff3d604a3dc5..3e98f995e8370f 100644
--- a/selfdrive/locationd/get_vp.c
+++ b/selfdrive/locationd/get_vp.c
@@ -10,8 +10,8 @@ int get_intersections(double *lines, double *intersections, long long n) {
       D = L1[0] * L2[1] - L1[1] * L2[0];
       Dx = L1[2] * L2[1] - L1[1] * L2[2];
       Dy = L1[0] * L2[2] - L1[2] * L2[0];
-      // only intersect lines from different quadrants
-      if ((D != 0) && (L1[0]*L2[0]*L1[1]*L2[1] < 0)){
+      // only intersect lines from different quadrants and only left-right crossing
+      if ((D != 0) && (L1[0]*L2[0]*L1[1]*L2[1] < 0) && (L1[0]*L2[0] < 0)){
         x = Dx / D;
         y = Dy / D;
         if ((0 < x) &&
diff --git a/selfdrive/loggerd/loggerd b/selfdrive/loggerd/loggerd
index 5b8722558660ba..1db19dae1cf737 100755
Binary files a/selfdrive/loggerd/loggerd and b/selfdrive/loggerd/loggerd differ
diff --git a/selfdrive/orbd/Makefile b/selfdrive/orbd/Makefile
index c33a8636bcefe8..32e9c6dfa549da 100644
--- a/selfdrive/orbd/Makefile
+++ b/selfdrive/orbd/Makefile
@@ -54,7 +54,7 @@ all: orbd
 
 include ../common/cereal.mk
 
-DEP_OBJS = ../common/visionipc.o ../common/swaglog.o $(PHONELIBS)/json/src/json.o
+DEP_OBJS = ../common/visionipc.o ../common/ipc.o ../common/swaglog.o $(PHONELIBS)/json/src/json.o
 
 orbd: orbd_dsp.o $(DEP_OBJS) calculator_stub.o freethedsp.o
 	@echo "[ LINK ] $@"
diff --git a/selfdrive/sensord/gpsd b/selfdrive/sensord/gpsd
index 077800d3b5629b..9d577af8d86734 100755
Binary files a/selfdrive/sensord/gpsd and b/selfdrive/sensord/gpsd differ
diff --git a/selfdrive/sensord/sensord b/selfdrive/sensord/sensord
index c5e8eab6c088ae..94cb27d1cb9dfe 100755
Binary files a/selfdrive/sensord/sensord and b/selfdrive/sensord/sensord differ
diff --git a/selfdrive/thermald.py b/selfdrive/thermald.py
index 35b2fc88ce90e4..614ec212a802bd 100755
--- a/selfdrive/thermald.py
+++ b/selfdrive/thermald.py
@@ -11,8 +11,10 @@
 from common.params import Params
 from common.realtime import sec_since_boot
 from common.numpy_fast import clip
+from common.filter_simple import FirstOrderFilter
 
 ThermalStatus = log.ThermalData.ThermalStatus
+CURRENT_TAU = 2.   # 2s time constant
 
 def read_tz(x):
   with open("/sys/devices/virtual/thermal/thermal_zone%d/temp" % x) as f:
@@ -152,11 +154,12 @@ def thermald_thread():
   passive_starter = LocationStarter()
   thermal_status = ThermalStatus.green
   health_sock.RCVTIMEO = 1500
+  current_filter = FirstOrderFilter(0., CURRENT_TAU, 1.)
 
   params = Params()
 
   while 1:
-    td = messaging.recv_sock(health_sock, wait=True)
+    health = messaging.recv_sock(health_sock, wait=True)
     location = messaging.recv_sock(location_sock)
     location = location.gpsLocation if location else None
     msg = read_thermal()
@@ -178,6 +181,9 @@ def thermald_thread():
     with open("/sys/class/power_supply/usb/online") as f:
       msg.thermal.usbOnline = bool(int(f.read()))
 
+    current_filter.update(msg.thermal.batteryCurrent / 1e6)
+    msg.thermal.chargerDisabled = current_filter.x > 1.0   # if current is ? 1A out, then charger might be off
+
     # TODO: add car battery voltage check
     max_cpu_temp = max(msg.thermal.cpu0, msg.thermal.cpu1,
                        msg.thermal.cpu2, msg.thermal.cpu3) / 10.0
@@ -209,11 +215,11 @@ def thermald_thread():
     # **** starting logic ****
 
     # start constellation of processes when the car starts
-    ignition = td is not None and td.health.started
+    ignition = health is not None and health.health.started
     ignition_seen = ignition_seen or ignition
 
     # add voltage check for ignition
-    if not ignition_seen and td is not None and td.health.voltage > 13500:
+    if not ignition_seen and health is not None and health.health.voltage > 13500:
       ignition = True
 
     do_uninstall = params.get("DoUninstall") == "1"
@@ -226,7 +232,7 @@ def thermald_thread():
     passive = (params.get("Passive") == "1")
 
     # start on gps movement if we haven't seen ignition and are in passive mode
-    should_start = should_start or (not (ignition_seen and td) # seen ignition and panda is connected
+    should_start = should_start or (not (ignition_seen and health) # seen ignition and panda is connected
                                     and passive
                                     and passive_starter.update(started_ts, location))
 
@@ -273,7 +279,7 @@ def thermald_thread():
     if (count%60) == 0:
       cloudlog.event("STATUS_PACKET",
         count=count,
-        health=(td.to_dict() if td else None),
+        health=(health.to_dict() if health else None),
         location=(location.to_dict() if location else None),
         thermal=msg.to_dict())
 
diff --git a/selfdrive/ui/Makefile b/selfdrive/ui/Makefile
index 4d6df2e45e4a2b..333a25b96ce192 100644
--- a/selfdrive/ui/Makefile
+++ b/selfdrive/ui/Makefile
@@ -38,6 +38,7 @@ CXXFLAGS += -DQCOM
 OBJS = ui.o \
        ../common/glutil.o \
        ../common/visionipc.o \
+       ../common/ipc.o \
        ../common/visionimg.o \
        ../common/visionbuf_ion.o \
        ../common/framebuffer.o \
diff --git a/selfdrive/ui/ui.c b/selfdrive/ui/ui.c
index ed8a05aeee013b..83244f339db0d4 100644
--- a/selfdrive/ui/ui.c
+++ b/selfdrive/ui/ui.c
@@ -38,11 +38,6 @@
 #include "bbuistate.h"
 //BB end
 
-// Calibration status values from controlsd.py
-#define CALIBRATION_UNCALIBRATED 0
-#define CALIBRATION_CALIBRATED 1
-#define CALIBRATION_INVALID 2
-
 #define STATUS_STOPPED 0
 #define STATUS_DISENGAGED 1
 #define STATUS_ENGAGED 2
@@ -148,10 +143,6 @@ typedef struct UIScene {
 
   uint64_t started_ts;
 
-  // Used to display calibration progress
-  int cal_status;
-  int cal_perc;
-
   // Used to show gps planner status
   bool gps_planner_active;
 
@@ -460,7 +451,6 @@ static void ui_init_vision(UIState *s, const VisionStreamBufs back_bufs,
   s->scene = (UIScene){
       .frontview = getenv("FRONTVIEW") != NULL,
       .fullview = getenv("FULLVIEW") != NULL,
-      .cal_status = CALIBRATION_CALIBRATED,
       .transformed_width = ui_info.transformed_width,
       .transformed_height = ui_info.transformed_height,
       .front_box_x = ui_info.front_box_x,
@@ -1116,16 +1106,6 @@ static void ui_draw_vision_alert(UIState *s, int va_size, int va_color,
   }
 }
 
-static void ui_draw_calibration_status(UIState *s) {
-  const UIScene *scene = &s->scene;
-  char calib_str1[64];
-  char calib_str2[64];
-  snprintf(calib_str1, sizeof(calib_str1), "Calibration in Progress: %d%%", scene->cal_perc);
-  snprintf(calib_str2, sizeof(calib_str2), (s->is_metric?"Drive above 35 km/h":"Drive above 15 mph"));
-
-  ui_draw_vision_alert(s, ALERTSIZE_MID, s->status, calib_str1, calib_str2);
-}
-
 static void ui_draw_vision(UIState *s) {
   //BB code added to only draw every other frame
   if (!s->b.shouldDrawFrame) {
@@ -1176,9 +1156,6 @@ static void ui_draw_vision(UIState *s) {
     // Controls Alerts
     ui_draw_vision_alert(s, s->scene.alert_size, s->status,
                             s->scene.alert_text1, s->scene.alert_text2);
-  } else if (scene->cal_status == CALIBRATION_UNCALIBRATED) {
-    // Calibration Status
-    ui_draw_calibration_status(s);
   } else {
     ui_draw_vision_footer(s);
   }
@@ -1549,9 +1526,6 @@ static void ui_update(UIState *s) {
         struct cereal_LiveCalibrationData datad;
         cereal_read_LiveCalibrationData(&datad, eventd.liveCalibration);
 
-        s->scene.cal_status = datad.calStatus;
-        s->scene.cal_perc = datad.calPerc;
-
         // should we still even have this?
         capn_list32 warpl = datad.warpMatrix2;
         capn_resolve(&warpl.p);  // is this a bug?
diff --git a/selfdrive/visiond/visiond b/selfdrive/visiond/visiond
index ca148f01693c4c..b26fbdeda38674 100755
Binary files a/selfdrive/visiond/visiond and b/selfdrive/visiond/visiond differ