openpilot v0.3.4 release

2017-07-28 01:24:39 -07:00 · 2017-07-28 01:24:39 -07:00 · 6f46f988d9
parent 68485aa4e4
commit 6f46f988d9
67 changed files with 1940 additions and 774 deletions
--- a/.gitignore
+++ b/.gitignore
@ -1,6 +1,7 @@
 .DS_Store
 .tags
 .ipynb_checkpoints
+.idea
 model2.png

 *.d
--- a/.gitmodules
+++ b/.gitmodules
@ -4,3 +4,6 @@
 [submodule "opendbc"]
 	path = opendbc
 	url = https://github.com/commaai/opendbc.git
+[submodule "pyextra"]
+	path = pyextra
+	url = https://github.com/commaai/openpilot-pyextra.git
--- a/Dockerfile.openpilot
+++ b/Dockerfile.openpilot
@ -1,15 +1,9 @@
-FROM ubuntu:14.04
+FROM ubuntu:16.04
 ENV PYTHONUNBUFFERED 1

-RUN apt-get update && apt-get install -y build-essential screen wget bzip2 git libglib2.0-0
+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

-RUN wget -nv -O /tmp/anaconda.sh https://repo.continuum.io/archive/Anaconda2-4.2.0-Linux-x86_64.sh && \
-  /bin/bash /tmp/anaconda.sh -b -p /opt/conda && \
-  rm /tmp/anaconda.sh
-
-ENV PATH /opt/conda/bin:$PATH
-
-RUN conda install numpy==1.11.2 && conda install scipy==0.18.1
+RUN pip install numpy==1.11.2 scipy==0.18.1 matplotlib

 COPY requirements_openpilot.txt /tmp/
 RUN pip install -r /tmp/requirements_openpilot.txt
--- a/README.md
+++ b/README.md
@ -101,6 +101,8 @@ Licensing

 openpilot is released under the MIT license.

+Any user of this software shall indemnify and hold harmless Comma.ai, Inc. and its directors, officers, employees, agents, stockholders, affiliates, subcontractors and customers from and against all allegations, claims, actions, suits, demands, damages, liabilities, obligations, losses, settlements, judgments, costs and expenses (including without limitation attorneys’ fees and costs) which arise out of, relate to or result from any use of this software by user.
+
 **THIS IS ALPHA QUALITY SOFTWARE FOR RESEARCH PURPOSES ONLY. THIS IS NOT A PRODUCT.
 YOU ARE RESPONSIBLE FOR COMPLYING WITH LOCAL LAWS AND REGULATIONS.
 NO WARRANTY EXPRESSED OR IMPLIED.**
--- a/RELEASES.md
+++ b/RELEASES.md
@ -1,3 +1,11 @@
+Version 0.3.4 (2017-07-28)
+==========================
+ * Improved model trained on more data
+ * Much improved controls tuning
+ * Performance improvements
+ * Bugfixes and improvements to calibration
+ * Driving log can play back video
+
 Version 0.3.3  (2017-06-28)
 ===========================
  * Improved model trained on more data
--- a/apk/com.baseui.apk
+++ b/apk/com.baseui.apk
--- a/cereal/init.py
+++ b/cereal/init.py
@ -1,8 +1,20 @@
 import os
 import capnp
-capnp.remove_import_hook()

 CEREAL_PATH = os.path.dirname(os.path.abspath(__file__))
-log = capnp.load(os.path.join(CEREAL_PATH, "log.capnp"))
-car = capnp.load(os.path.join(CEREAL_PATH, "car.capnp"))
+capnp.remove_import_hook()

+if os.getenv("NEWCAPNP"):
+  import tempfile
+  import pyximport
+
+  importers = pyximport.install(build_dir=os.path.join(tempfile.gettempdir(), ".pyxbld"))
+  try:
+    import cereal.gen.cython.log_capnp_cython as log
+    import cereal.gen.cython.car_capnp_cython as car
+  finally:
+    pyximport.uninstall(*importers)
+    del importers
+else:
+  log = capnp.load(os.path.join(CEREAL_PATH, "log.capnp"))
+  car = capnp.load(os.path.join(CEREAL_PATH, "car.capnp"))
--- a/cereal/build_from_src.mk
+++ b/cereal/build_from_src.mk
@ -1,14 +1,21 @@
 SRCS := log.capnp car.capnp

-GENS := gen/c/car.capnp.c gen/c/log.capnp.c gen/c/c++.capnp.h gen/c/java.capnp.h \
-	      gen/cpp/car.capnp.c++ gen/cpp/log.capnp.c++
+GENS := gen/cpp/car.capnp.c++ gen/cpp/log.capnp.c++
+
+
+UNAME_M := $(shell uname -m)
+
+# only generate C++ for docker tests
+ifneq ($(OPTEST),1)
+	GENS += gen/c/car.capnp.c gen/c/log.capnp.c gen/c/c++.capnp.h gen/c/java.capnp.h

 # Dont build java on the phone...
-UNAME_M := $(shell uname -m)
 ifeq ($(UNAME_M),x86_64)
 	GENS += gen/java/Car.java gen/java/Log.java
 endif

+endif
+
 .PHONY: all
 all: $(GENS)

--- a/cereal/car.capnp
+++ b/cereal/car.capnp
@ -50,6 +50,7 @@ struct CarState {
  struct CruiseState {
    enabled @0: Bool;
    speed @1: Float32;
+    mainOn @2: Bool;
  }

  enum Error {
@ -63,7 +64,7 @@ struct CarState {
    seatbeltNotLatched @6;
    espDisabled @7;
    wrongCarMode @8;
-    steerTemporarilyUnavailable @9;
+    steerTempUnavailable @9;
    reverseGear @10;
    # ...
  }
--- a/cereal/log.capnp
+++ b/cereal/log.capnp
@ -140,12 +140,16 @@ struct SensorEventData {
  sensor @1 :Int32;
  type @2 :Int32;
  timestamp @3 :Int64;
+  uncalibratedDEPRECATED @10 :Bool;
+
  union {
    acceleration @4 :SensorVec;
    magnetic @5 :SensorVec;
    orientation @6 :SensorVec;
    gyro @7 :SensorVec;
    pressure @9 :SensorVec;
+    magneticUncalibrated @11 :SensorVec;
+    gyroUncalibrated @12 :SensorVec;
  }
  source @8 :SensorSource;

@ -182,7 +186,7 @@ struct GpsLocationData {
  # Represents heading in degrees.
  bearing @5 :Float32;

-  # Represents expected accuracy in meters.
+  # Represents expected accuracy in meters. (presumably 1 sigma?)
  accuracy @6 :Float32;

  # Timestamp for the location fix.
@ -190,6 +194,18 @@ struct GpsLocationData {
  timestamp @7 :Int64;

  source @8 :SensorSource;
+  
+  # Represents NED velocity in m/s.
+  vNED @9 :List(Float32);
+  
+  # Represents expected vertical accuracy in meters. (presumably 1 sigma?)
+  verticalAccuracy @10 :Float32;
+
+  # Represents bearing accuracy in degrees. (presumably 1 sigma?)
+  bearingAccuracy @11 :Float32;
+
+  # Represents velocity accuracy in m/s. (presumably 1 sigma?)
+  speedAccuracy @12 :Float32;

  enum SensorSource {
    android @0;
@ -198,6 +214,7 @@ struct GpsLocationData {
    velodyne @3;  # Velodyne IMU
    fusion @4;
    external @5;
+    ublox @6;
  }
 }

@ -205,7 +222,7 @@ struct CanData {
  address @0 :UInt32;
  busTime @1 :UInt16;
  dat     @2 :Data;
-  src     @3 :Int8;
+  src     @3 :UInt8;
 }

 struct ThermalData {
@ -243,7 +260,8 @@ struct LiveUI {
 struct Live20Data {
  canMonoTimes @10 :List(UInt64);
  mdMonoTime @6 :UInt64;
-  ftMonoTime @7 :UInt64;
+  ftMonoTimeDEPRECATED @7 :UInt64;
+  l100MonoTime @11 :UInt64;

  # all deprecated
  warpMatrixDEPRECATED @0 :List(Float32);
@ -296,10 +314,11 @@ struct LiveTracks {
 }

 struct Live100Data {
-  canMonoTime @16 :UInt64;
+  canMonoTimeDEPRECATED @16 :UInt64;
  canMonoTimes @21 :List(UInt64);
  l20MonoTimeDEPRECATED @17 :UInt64;
  mdMonoTimeDEPRECATED @18 :UInt64;
+  planMonoTime @28 :UInt64;

  vEgo @0 :Float32;
  aEgoDEPRECATED @1 :Float32;
@ -393,7 +412,8 @@ struct EncodeIndex {
    bigBoxLossless @0;   # rcamera.mkv
    fullHEVC @1;         # fcamera.hevc
    bigBoxHEVC @2;       # bcamera.hevc
-    chffrAndroidH264 @3; # camera
+    chffrAndroidH264 @3; # acamera
+    fullLosslessClip @4; # prcamera.mkv
  }
 }

@ -413,6 +433,9 @@ struct LogRotate {
 }

 struct Plan {
+  mdMonoTime @9 :UInt64;
+  l20MonoTime @10 :UInt64;
+
  # lateral, 3rd order polynomial
  lateralValid @0: Bool;
  dPoly @1 :List(Float32);
@ -424,6 +447,7 @@ struct Plan {
  aTargetMax @5 :Float32;
  jerkFactor @6 :Float32;
  hasLead @7 :Bool;
+  fcw @8 :Bool;
 }

 struct LiveLocationData {
@ -909,6 +933,114 @@ struct ProcLog {

 }

+struct UbloxGnss {
+  union {
+    measurementReport @0 :MeasurementReport;
+    ephemeris @1 :Ephemeris;
+  }
+
+  struct MeasurementReport {
+    #received time of week in gps time in seconds and gps week
+    rcvTow @0 :Float64;
+    gpsWeek @1 :UInt16;
+    # leap seconds in seconds
+    leapSeconds @2 :UInt16;
+    # receiver status
+    receiverStatus @3 :ReceiverStatus;
+    # num of measurements to follow
+    numMeas @4 :UInt8;
+    measurements @5 :List(Measurement);
+        
+    struct ReceiverStatus {
+      # leap seconds have been determined 
+      leapSecValid @0 :Bool;
+      # Clock reset applied
+      clkReset @1 : Bool;
+    }
+
+    struct Measurement {
+      svId @0 :UInt8;
+      trackingStatus @1 :TrackingStatus;
+      # pseudorange in meters
+      pseudorange @2 :Float64;
+      # carrier phase measurement in cycles
+      carrierCycles @3 :Float64;
+      # doppler measurement in Hz
+      doppler @4 :Float32;
+      # GNSS id, 0 is gps
+      gnssId @5 :UInt8;
+      glonassFrequencyIndex @6 :UInt8;
+      # carrier phase locktime counter in ms
+      locktime @7 :UInt16;
+      # Carrier-to-noise density ratio (signal strength) in dBHz
+      cno @8 : UInt8;
+      # pseudorange standard deviation in meters
+      pseudorangeStdev @9 :Float32;
+      # carrier phase standard deviation in cycles
+      carrierPhaseStdev @10 :Float32;
+      # doppler standard deviation in Hz
+      dopplerStdev @11 :Float32;
+
+      struct TrackingStatus {
+        # pseudorange valid
+        pseudorangeValid @0 :Bool;
+        # carrier phase valid
+        carrierPhaseValid @1 :Bool;
+        # half cycle valid
+        halfCycleValid @2 :Bool;
+        # half sycle subtracted from phase
+        halfCycleSubtracted @3 :Bool;
+      }
+    }
+  }
+
+  struct Ephemeris {
+    # This is according to the rinex (2?) format
+    svId @0 :UInt16;
+    year @1 :UInt16;
+    month @2 :UInt16;
+    day @3 :UInt16;
+    hour @4 :UInt16;
+    minute @5 :UInt16;
+    second @6 :Float32;
+    af0 @7 :Float64;
+    af1 @8 :Float64;
+    af2 @9 :Float64;
+
+    iode @10 :Float64;
+    crs @11 :Float64;
+    deltaN @12 :Float64;
+    m0 @13 :Float64;
+
+    cuc @14 :Float64;
+    ecc @15 :Float64;
+    cus @16 :Float64;
+    a @17 :Float64; # note that this is not the root!!
+    
+    toe @18 :Float64;
+    cic @19 :Float64;
+    omega0 @20 :Float64;
+    cis @21 :Float64;
+    
+    i0 @22 :Float64;
+    crc @23 :Float64;
+    omega @24 :Float64;
+    omegaDot @25 :Float64;
+    
+    iDot @26 :Float64;
+    codesL2 @27 :Float64;
+    gpsWeek @28 :Float64;
+    l2 @29 :Float64;
+    
+    svAcc @30 :Float64;
+    svHealth @31 :Float64;
+    tgd @32 :Float64;
+    iodc @33 :Float64;
+    
+    transmissionTime @34 :Float64;
+    fitInterval @35 :Float64;
+  }
+}
 struct Event {
  # in nanoseconds?
  logMonoTime @0 :UInt64;
@ -947,5 +1079,6 @@ struct Event {
    qcomGnss @31 :QcomGnss;
    lidarPts @32 :LidarPts;
    procLog @33 :ProcLog;
+    ubloxGnss @34 :UbloxGnss;
  }
 }
--- a/common/dbc.py
+++ b/common/dbc.py
@ -1,6 +1,7 @@
 import re
-import bitstring
+import os
 import struct
+import bitstring
 from collections import namedtuple

 def int_or_float(s):
@ -16,6 +17,7 @@ DBCSignal = namedtuple(

 class dbc(object):
  def __init__(self, fn):
+    self.name, _ = os.path.splitext(os.path.basename(fn))
    with open(fn) as f:
      self.txt = f.read().split("\n")
    self._warned_addresses = set()
@ -32,11 +34,8 @@ class dbc(object):
    # signals is a list of DBCSignal in order of increasing start_bit.
    self.msgs = {}

-    self.bits = []
-    for i in range(0, 64, 8):
-      for j in range(7, -1, -1):
-        self.bits.append(i+j)
-    self.bits_index = dict(zip(self.bits, range(64)))
+    # lookup to bit reverse each byte
+    self.bits_index = [(i & ~0b111) + ((-i-1) & 0b111) for i in xrange(64)]

    for l in self.txt:
      l = l.strip()
--- a/common/fingerprints.py
+++ b/common/fingerprints.py
@ -1,3 +1,4 @@
+import os

 _FINGERPRINTS = {
  "ACURA ILX 2016 ACURAWATCH PLUS": {
@ -51,30 +52,13 @@ def fingerprint(logcan):
  import selfdrive.messaging as messaging
  from cereal import car
  from common.realtime import sec_since_boot
-  import os
+
  if os.getenv("SIMULATOR") is not None or logcan is None:
-    # send message
-    ret = car.CarParams.new_message()
-
-    ret.carName = "simulator"
-    ret.radarName = "nidec"
-    ret.carFingerprint = "THE LOW QUALITY SIMULATOR"
-
-    ret.enableSteer = True
-    ret.enableBrake = True
-    ret.enableGas = True
-    ret.enableCruise = False
-
-    ret.wheelBase = 2.67
-    ret.steerRatio = 15.3
-    ret.slipFactor = 0.0014
-
-    ret.steerKp, ret.steerKi = 12.0, 1.0
-    return ret
+    return ("simulator", None)
+  elif os.getenv("SIMULATOR2") is not None:
+    return ("simulator2", None)

  print "waiting for fingerprint..."
-  brake_only = True
-
  candidate_cars = all_known_cars()
  finger = {}
  st = None
@ -83,9 +67,6 @@ def fingerprint(logcan):
      if st is None:
        st = sec_since_boot()
      for can in a.can:
-        # pedal
-        if can.address == 0x201 and can.src == 0:
-          brake_only = False
        if can.src == 0:
          finger[can.address] = len(can.dat)
        candidate_cars = eliminate_incompatible_cars(can, candidate_cars)
@ -98,37 +79,4 @@ def fingerprint(logcan):
      raise Exception("car doesn't match any fingerprints")

  print "fingerprinted", candidate_cars[0]
-
-  # send message
-  ret = car.CarParams.new_message()
-
-  ret.carName = "honda"
-  ret.radarName = "nidec"
-  ret.carFingerprint = candidate_cars[0]
-
-  ret.enableSteer = True
-  ret.enableBrake = True
-  ret.enableGas = not brake_only
-  ret.enableCruise = brake_only
-  #ret.enableCruise = False
-
-  ret.wheelBase = 2.67
-  ret.steerRatio = 15.3
-  ret.slipFactor = 0.0014
-
-  if candidate_cars[0] == "HONDA CIVIC 2016 TOURING":
-    ret.steerKp, ret.steerKi = 12.0, 1.0
-  elif candidate_cars[0] == "ACURA ILX 2016 ACURAWATCH PLUS":
-    if not brake_only:
-      # assuming if we have an interceptor we also have a torque mod
-      ret.steerKp, ret.steerKi = 6.0, 0.5
-    else:
-      ret.steerKp, ret.steerKi = 12.0, 1.0
-  elif candidate_cars[0] == "HONDA ACCORD 2016 TOURING":
-    ret.steerKp, ret.steerKi = 12.0, 1.0
-  elif candidate_cars[0] == "HONDA CR-V 2016 TOURING":
-    ret.steerKp, ret.steerKi = 6.0, 0.5
-  else:
-    raise ValueError("unsupported car %s" % candidate_cars[0])
-
-  return ret
+  return (candidate_cars[0], finger)
--- a/common/kalman/ekf.py
+++ b/common/kalman/ekf.py
@ -1,7 +1,6 @@
 import abc
 import numpy as np
 import numpy.matlib
-
 # The EKF class contains the framework for an Extended Kalman Filter, but must be subclassed to use.
 # A subclass must implement:
 #   1) calc_transfer_fun(); see bottom of file for more info.
@ -68,55 +67,6 @@ class SimpleSensor:
      self.covar = covar
    return SensorReading(data, self.covar, self.obs_model)

-class GPS:
-  earth_r = 6371e3  # m, average earth radius
-
-  def __init__(self, xy_idx=(0, 1), dims=2, var=1e4):
-    self.obs_model = np.matlib.zeros((2, dims))
-    self.obs_model[:, tuple(xy_idx)] = np.matlib.identity(2)
-    self.covar = np.matlib.identity(2) * var
-
-  # [lat, lon] in decimal degrees
-  def init_pos(self, latlon):
-    self.init_lat, self.init_lon = np.radians(np.asarray(latlon[:2]))
-
-  # Compute straight-line distance, in meters, between two lat/long coordinates
-  # Input in radians
-  def haversine(self, lat1, lon1, lat2, lon2):
-    lat_diff = lat2 - lat1
-    lon_diff = lon2 - lon1
-    d = np.sin(lat_diff * 0.5)**2 + np.cos(lat1) * np.cos(lat2) * np.sin(
-      lon_diff * 0.5)**2
-    h = 2 * GPS.earth_r * np.arcsin(np.sqrt(d))
-    return h
-
-  # Convert decimal degrees into meters
-  def convert_deg2m(self, lat, lon):
-    lat, lon = np.radians([lat, lon])
-
-    xs = (lon - self.init_lon) * np.cos(self.init_lat) * GPS.earth_r
-    ys = (lat - self.init_lat) * GPS.earth_r
-
-    return xs, ys
-
-  # Convert meters into decimal degrees
-  def convert_m2deg(self, xs, ys):
-    lat = ys / GPS.earth_r + self.init_lat
-    lon = xs / (GPS.earth_r * np.cos(self.init_lat)) + self.init_lon
-
-    return np.degrees(lat), np.degrees(lon)
-
-  # latlon is [lat, long,] as decimal degrees
-  # accuracy is as given by Android location service: radius of 68% confidence
-  def read(self, latlon, accuracy=None):
-    x_dist, y_dist = self.convert_deg2m(latlon[0], latlon[1])
-    if not accuracy:
-      covar = self.covar
-    else:
-      covar = np.matlib.identity(2) * accuracy**2
-
-    return SensorReading(
-      np.asmatrix([x_dist, y_dist]).T, covar, self.obs_model)

 class EKF:
  __metaclass__ = abc.ABCMeta
@ -224,7 +174,7 @@ class EKF:

    #! Clip covariance to avoid explosions
    self.covar = np.clip(self.covar,-1e10,1e10)
-
+    
  @abc.abstractmethod
  def calc_transfer_fun(self, dt):
    """Return a tuple with the transfer function and transfer function jacobian
--- a/common/logging_extra.py
+++ b/common/logging_extra.py
@ -16,6 +16,10 @@ def json_handler(obj):
 def json_robust_dumps(obj):
  return json.dumps(obj, default=json_handler)

+class NiceOrderedDict(OrderedDict):
+  def __str__(self):
+    return '{'+', '.join("%r: %r" % p for p in self.iteritems())+'}'
+
 class SwagFormatter(logging.Formatter):
  def __init__(self, swaglogger):
    logging.Formatter.__init__(self, None, '%a %b %d %H:%M:%S %Z %Y')
@ -24,7 +28,7 @@ class SwagFormatter(logging.Formatter):
    self.host = socket.gethostname()

  def format_dict(self, record):
-    record_dict = OrderedDict()
+    record_dict = NiceOrderedDict()

    if isinstance(record.msg, dict):
      record_dict['msg'] = record.msg
@ -119,7 +123,7 @@ class SwagLogger(logging.Logger):
    self.global_ctx.update(kwargs)

  def event(self, event_name, *args, **kwargs):
-    evt = OrderedDict()
+    evt = NiceOrderedDict()
    evt['event'] = event_name
    if args:
      evt['args'] = args
--- a/common/realtime.py
+++ b/common/realtime.py
@ -29,7 +29,7 @@ libc = ffi.dlopen(None)
 CLOCK_MONOTONIC_RAW = 4
 CLOCK_BOOTTIME = 7

-if hasattr(libc, 'clock_gettime'):
+if platform.system() != 'Darwin' and hasattr(libc, 'clock_gettime'):
  c_clock_gettime = libc.clock_gettime

  tlocal = threading.local()
--- a/2
+++ b/2
@ -1 +1 @@
-Subproject commit d584cdd46ca398aace9f1a7ee6c967a8c7602e8e
+Subproject commit b77861eb00d45e25af501f78bbed155d2df06159
--- a/1
+++ b/1
@ -0,0 +1 @@
+Subproject commit e0738376db27d208603d7e63dd465e003ca06325
--- a/requirements_openpilot.txt
+++ b/requirements_openpilot.txt
@ -9,4 +9,6 @@ requests==2.10.0
 setproctitle==1.1.10
 simplejson==3.8.2
 pyyaml==3.12
+cffi==1.7.0
+enum34==1.1.1
 -e git+https://github.com/commaai/le_python.git#egg=Logentries
--- a/selfdrive/boardd/.gitignore
+++ b/selfdrive/boardd/.gitignore
@ -0,0 +1 @@
+boardd
--- a/selfdrive/boardd/boardd.cc
+++ b/selfdrive/boardd/boardd.cc
@ -31,11 +31,16 @@ pthread_mutex_t usb_lock;

 bool spoofing_started = false;
 bool fake_send = false;
+bool loopback_can = false;

 // double the FIFO size
 #define RECV_SIZE (0x1000)
 #define TIMEOUT 0

+#define SAFETY_NOOUTPUT  0x0000
+#define SAFETY_HONDA     0x0001
+#define SAFETY_ALLOUTPUT 0x1337
+
 bool usb_connect() {
  int err;

@ -48,22 +53,36 @@ bool usb_connect() {
  err = libusb_claim_interface(dev_handle, 0);
  if (err != 0) { return false; }

+  if (loopback_can) {
+    libusb_control_transfer(dev_handle, 0xc0, 0xe5, 1, 0, NULL, 0, TIMEOUT);
+  }
+
  // power off ESP
  libusb_control_transfer(dev_handle, 0xc0, 0xd9, 0, 0, NULL, 0, TIMEOUT);

  // forward CAN1 to CAN3...soon
  //libusb_control_transfer(dev_handle, 0xc0, 0xdd, 1, 2, NULL, 0, TIMEOUT);
-  
+
  // set UART modes for Honda Accord
-  for (int uart = 2; uart <= 3; uart++) {
+  /*for (int uart = 2; uart <= 3; uart++) {
    // 9600 baud
-    libusb_control_transfer(dev_handle, 0xc0, 0xe1, uart, 9600, NULL, 0, TIMEOUT);
+    libusb_control_transfer(dev_handle, 0x40, 0xe1, uart, 9600, NULL, 0, TIMEOUT);
    // even parity
-    libusb_control_transfer(dev_handle, 0xc0, 0xe2, uart, 1, NULL, 0, TIMEOUT);
+    libusb_control_transfer(dev_handle, 0x40, 0xe2, uart, 1, NULL, 0, TIMEOUT);
    // callback 1
-    libusb_control_transfer(dev_handle, 0xc0, 0xe3, uart, 1, NULL, 0, TIMEOUT);
+    libusb_control_transfer(dev_handle, 0x40, 0xe3, uart, 1, NULL, 0, TIMEOUT);
  }

+  // TODO: Boardd should be able to set the baud rate
+  int baud = 500000;
+  libusb_control_transfer(dev_handle, 0x40, 0xde, 0, 0,
+                          (unsigned char *)&baud, sizeof(baud), TIMEOUT); // CAN1
+  libusb_control_transfer(dev_handle, 0x40, 0xde, 1, 0,
+                          (unsigned char *)&baud, sizeof(baud), TIMEOUT); // CAN2*/
+
+  // TODO: Boardd should be able to be told which safety model to use
+  libusb_control_transfer(dev_handle, 0x40, 0xdc, SAFETY_HONDA, 0, NULL, 0, TIMEOUT);
+
  return true;
 }

@ -90,7 +109,7 @@ void can_recv(void *s) {

  // do recv
  pthread_mutex_lock(&usb_lock);
- 
+
  do {
    err = libusb_bulk_transfer(dev_handle, 0x81, (uint8_t*)data, RECV_SIZE, &recv, TIMEOUT);
    if (err != 0) { handle_usb_issue(err, __func__); }
@ -127,13 +146,13 @@ void can_recv(void *s) {
    canData[i].setBusTime(data[i*4+1] >> 16);
    int len = data[i*4+1]&0xF;
    canData[i].setDat(kj::arrayPtr((uint8_t*)&data[i*4+2], len));
-    canData[i].setSrc((data[i*4+1] >> 4) & 0xf);
+    canData[i].setSrc((data[i*4+1] >> 4) & 0xff);
  }

  // send to can
  auto words = capnp::messageToFlatArray(msg);
  auto bytes = words.asBytes();
-  zmq_send(s, bytes.begin(), bytes.size(), 0); 
+  zmq_send(s, bytes.begin(), bytes.size(), 0);
 }

 void can_health(void *s) {
@ -181,7 +200,7 @@ void can_health(void *s) {
  // send to health
  auto words = capnp::messageToFlatArray(msg);
  auto bytes = words.asBytes();
-  zmq_send(s, bytes.begin(), bytes.size(), 0); 
+  zmq_send(s, bytes.begin(), bytes.size(), 0);
 }


@ -316,6 +335,10 @@ int main() {
    fake_send = true;
  }

+  if(getenv("BOARDD_LOOPBACK")){
+    loopback_can = true;
+  }
+
  // init libusb
  err = libusb_init(&ctx);
  assert(err == 0);
@ -357,4 +380,3 @@ int main() {
  libusb_close(dev_handle);
  libusb_exit(ctx);
 }
-
--- a/selfdrive/boardd/boardd.py
+++ b/selfdrive/boardd/boardd.py
@ -18,6 +18,10 @@ except Exception:

 # TODO: rewrite in C to save CPU

+SAFETY_NOOUTPUT = 0
+SAFETY_HONDA = 1
+SAFETY_ALLOUTPUT = 0x1337
+
 # *** serialization functions ***
 def can_list_to_can_capnp(can_msgs, msgtype='can'):
  dat = messaging.new_message()
@ -85,6 +89,7 @@ def can_recv():

 def can_init():
  global handle, context
+  handle = None
  cloudlog.info("attempting can init")

  context = usb1.USBContext()
@ -94,6 +99,7 @@ def can_init():
    if device.getVendorID() == 0xbbaa and device.getProductID() == 0xddcc:
      handle = device.open()
      handle.claimInterface(0)
+      handle.controlWrite(0x40, 0xdc, SAFETY_HONDA, 0, b'')

  if handle is None:
    print "CAN NOT FOUND"
@ -190,4 +196,3 @@ def main(gctx=None):

 if __name__ == "__main__":
  main()
-
--- a/selfdrive/can/Makefile
+++ b/selfdrive/can/Makefile
@ -0,0 +1,72 @@
+CC = clang
+CXX = clang++
+
+PHONELIBS := ../../phonelibs
+
+UNAME_S := $(shell uname -s)
+UNAME_M := $(shell uname -m)
+
+WARN_FLAGS = -Werror=implicit-function-declaration \
+             -Werror=incompatible-pointer-types \
+             -Werror=int-conversion \
+             -Werror=return-type \
+             -Werror=format-extra-args \
+             -Wno-deprecated-declarations
+
+CFLAGS = -std=gnu11 -g -fPIC -O2 $(WARN_FLAGS)
+CXXFLAGS = -std=c++11 -g -fPIC -O2 $(WARN_FLAGS)
+
+ifeq ($(UNAME_S),Darwin)
+	CEREAL_LIBS := -L /usr/local/lib -lkj -lcapnp
+	ZMQ_LIBS = -L/usr/local/lib -lzmq
+else ifeq ($(OPTEST),1)
+	ZMQ_LIBS = -lzmq
+	CEREAL_LIBS = -lcapnp -lkj
+else ifeq ($(UNAME_M),x86_64)
+	EXTERNAL := ../../external
+	ZMQ_FLAGS = -I$(EXTERNAL)/zmq/include
+	ZMQ_LIBS = -L$(EXTERNAL)/zmq/lib -l:libzmq.a
+	CEREAL_CXXFLAGS := -I$(EXTERNAL)/capnp/include
+	CEREAL_LIBS := -L$(EXTERNAL)/capnp/lib -l:libcapnp.a -l:libkj.a
+else ifeq ($(UNAME_M),aarch64)
+	ZMQ_FLAGS = -I$(PHONELIBS)/zmq/aarch64/include
+	ZMQ_LIBS = -L$(PHONELIBS)/zmq/aarch64/lib -l:libzmq.a
+endif
+
+OPENDBC_PATH := $(shell python -c 'import opendbc; print opendbc.DBC_PATH')
+
+DBC_SOURCES := $(wildcard $(OPENDBC_PATH)/*.dbc)
+DBC_CCS := $(patsubst $(OPENDBC_PATH)/%.dbc,dbc_out/%.cc,$(DBC_SOURCES))
+
+CWD := $(shell pwd)
+
+.PHONY: all
+all: libdbc.so
+
+ifeq ($(UNAME_M),aarch64)
+include ../common/cereal.mk
+endif
+
+# make sure cereal is built
+libdbc.so:: ../../cereal/gen/cpp/log.capnp.h
+
+../../cereal/gen/cpp/log.capnp.h:
+	cd ../../cereal && make
+
+libdbc.so:: parser.cc $(DBC_CCS)
+	$(CXX) -fPIC -shared -o '$@' $^ \
+	  -I. \
+	  -I../.. \
+    $(CXXFLAGS) \
+    $(ZMQ_FLAGS) \
+    $(ZMQ_LIBS) \
+    $(CEREAL_CXXFLAGS) \
+    $(CEREAL_LIBS)
+
+dbc_out/%.cc: $(OPENDBC_PATH)/%.dbc
+	PYTHONPATH=$(PYTHONPATH):$(CWD)/../../pyextra ./process_dbc.py '$<' '$@'
+
+.PHONY: clean
+clean:
+	rm -rf libdbc.so*
+	rm -f dbc_out/*.cc
--- a/selfdrive/can/init.py
+++ b/selfdrive/can/init.py
--- a/selfdrive/can/dbc_out/.gitignore
+++ b/selfdrive/can/dbc_out/.gitignore
@ -0,0 +1,2 @@
+*.cc
+
--- a/selfdrive/can/dbc_out/.gitkeep
+++ b/selfdrive/can/dbc_out/.gitkeep
--- a/selfdrive/can/dbc_template.cc
+++ b/selfdrive/can/dbc_template.cc
@ -0,0 +1,51 @@
+#include <cstdint>
+
+#include "parser_common.h"
+
+namespace {
+
+{% for address, msg_name, sigs in msgs %}
+const Signal sigs_{{address}}[] = {
+  {% for sig in sigs %}
+    {
+      {% set b1 = (sig.start_bit//8)*8  + (-sig.start_bit-1) % 8 %}
+      .name = "{{sig.name}}",
+      .b1 = {{b1}},
+      .b2 = {{sig.size}},
+      .bo = {{64 - (b1 + sig.size)}},
+      .is_signed = {{"true" if sig.is_signed else "false"}},
+      .factor = {{sig.factor}},
+      .offset = {{sig.offset}},
+      {% if checksum_type == "honda" and sig.name == "CHECKSUM" %}
+      .type = SignalType::HONDA_CHECKSUM,
+      {% elif checksum_type == "honda" and sig.name == "COUNTER" %}
+      .type = SignalType::HONDA_COUNTER,
+      {% else %}
+      .type = SignalType::DEFAULT,
+      {% endif %}
+    },
+  {% endfor %}
+};
+{% endfor %}
+
+const Msg msgs[] = {
+{% for address, msg_name, sigs in msgs %}
+  {% set address_hex = "0x%X" % address %}
+  {
+    .name = "{{msg_name}}",
+    .address = {{address_hex}},
+    .num_sigs = ARRAYSIZE(sigs_{{address}}),
+    .sigs = sigs_{{address}},
+  },
+{% endfor %}
+};
+
+}
+
+const DBC {{dbc.name}} = {
+  .name = "{{dbc.name}}",
+  .num_msgs = ARRAYSIZE(msgs),
+  .msgs = msgs,
+};
+
+dbc_init({{dbc.name}})
--- a/selfdrive/can/parser.cc
+++ b/selfdrive/can/parser.cc
@ -0,0 +1,441 @@
+#include <cstdio>
+#include <cstdint>
+#include <cassert>
+#include <cstring>
+
+#include <unistd.h>
+#include <fcntl.h>
+#include <sys/stat.h>
+#include <sys/mman.h>
+
+#include <string>
+#include <vector>
+#include <algorithm>
+#include <unordered_map>
+
+#include <zmq.h>
+
+#include <capnp/serialize.h>
+#include "cereal/gen/cpp/log.capnp.h"
+
+#include "parser_common.h"
+
+#define DEBUG(...)
+// #define DEBUG printf
+#define INFO printf
+
+
+#define MAX_BAD_COUNTER 5
+
+namespace {
+
+uint64_t read_u64_be(const uint8_t* v) {
+  return (((uint64_t)v[0] << 56)
+          | ((uint64_t)v[1] << 48)
+          | ((uint64_t)v[2] << 40)
+          | ((uint64_t)v[3] << 32)
+          | ((uint64_t)v[4] << 24)
+          | ((uint64_t)v[5] << 16)
+          | ((uint64_t)v[6] << 8)
+          | (uint64_t)v[7]);
+}
+
+std::vector<const DBC*> g_dbc;
+
+bool honda_checksum(int address, uint64_t d, int l) {
+  int target = (d >> l) & 0xF;
+
+  DEBUG("check checksum %16lx %d", d, l);
+
+  // remove checksum from calculation
+  d &= ~(0xFLL << l);
+
+  int s = 0;
+  while (address > 0) { s += (address & 0xF); address >>= 4; }
+  while (d > 0) { s += (d & 0xF); d >>= 4; }
+  s = 8-s;
+  s &= 0xF;
+
+  DEBUG("   %d = %d\n", target, s);
+  return target == s;
+}
+
+struct MessageState {
+  uint32_t address;
+
+  std::vector<Signal> parse_sigs;
+  std::vector<double> vals;
+
+  uint64_t seen;
+  uint64_t check_threshold;
+
+  uint8_t counter;
+  uint8_t counter_fail;
+
+  bool parse(uint64_t sec, uint64_t dat) {
+    for (int i=0; i < parse_sigs.size(); i++) {
+      auto& sig = parse_sigs[i];
+
+      int64_t tmp = (dat >> sig.bo) & ((1 << sig.b2)-1);
+      if (sig.is_signed) {
+        tmp -= (tmp >> (sig.b2-1)) ? (1<<sig.b2) : 0; //signed
+      }
+
+      DEBUG("parse %X %s -> %ld\n", address, sig.name, tmp);
+
+      if (sig.type == SignalType::HONDA_CHECKSUM) {
+        if (!honda_checksum(address, dat, sig.bo)) {
+          INFO("%X CHECKSUM FAIL\n", address);
+          return false;
+        }
+      } else if (sig.type == SignalType::HONDA_COUNTER) {
+        if (!honda_update_counter(tmp)) {
+          return false;
+        }
+      }
+
+      vals[i] = tmp * sig.factor + sig.offset;
+    }
+    seen = sec;
+
+    return true;
+  }
+
+
+  bool honda_update_counter(int64_t v) {
+    uint8_t old_counter = counter;
+    counter = v;
+    if (((old_counter+1) & 3) != v) {
+      counter_fail += 1;
+      if (counter_fail > 1) {
+        INFO("%X COUNTER FAIL %d -- %d vs %d\n", address, counter_fail, old_counter, (int)v);
+      }
+      if (counter_fail >= MAX_BAD_COUNTER) {
+        return false;
+      }
+    } else if (counter_fail > 0) {
+      counter_fail--;
+    }
+    return true;
+  }
+
+};
+
+class CANParser {
+ public:
+  CANParser(int abus, const std::string& dbc_name,
+            const std::vector<MessageParseOptions> &options,
+            const std::vector<SignalParseOptions> &sigoptions)
+    : bus(abus) {
+    // connect to can on 8006
+    context = zmq_ctx_new();
+    subscriber = zmq_socket(context, ZMQ_SUB);
+    zmq_setsockopt(subscriber, ZMQ_SUBSCRIBE, "", 0);
+    zmq_connect(subscriber, "tcp://127.0.0.1:8006");
+
+    for (auto dbci : g_dbc) {
+      if (dbci->name == dbc_name) {
+        dbc = dbci;
+        break;
+      }
+    }
+    assert(dbc);
+
+    for (auto &op : options) {
+      MessageState state = {
+        .address = op.address,
+        // .check_frequency = op.check_frequency,
+      };
+
+      // msg is not valid if a message isn't received for 10 consecutive steps
+      if (op.check_frequency > 0) {
+        state.check_threshold = (1000000000ULL / op.check_frequency) * 10;
+      }
+
+
+      const Msg* msg = NULL;
+      for (int i=0; i<dbc->num_msgs; i++) {
+        if (dbc->msgs[i].address == op.address) {
+          msg = &dbc->msgs[i];
+          break;
+        }
+      }
+      if (!msg) {
+        fprintf(stderr, "CANParser: could not find message 0x%X in dnc %s\n", op.address, dbc_name.c_str());
+        assert(false);
+      }
+
+      // track checksums and for this message
+      for (int i=0; i<msg->num_sigs; i++) {
+        const Signal *sig = &msg->sigs[i];
+        if (sig->type == HONDA_CHECKSUM
+            || sig->type == HONDA_COUNTER) {
+          state.parse_sigs.push_back(*sig);
+          state.vals.push_back(0);
+        }
+      }
+
+      // track requested signals for this message
+      for (auto &sigop : sigoptions) {
+        if (sigop.address != op.address) continue;
+
+        for (int i=0; i<msg->num_sigs; i++) {
+          const Signal *sig = &msg->sigs[i];
+          if (strcmp(sig->name, sigop.name) == 0
+              && sig->type != HONDA_CHECKSUM
+              && sig->type != HONDA_COUNTER) {
+            state.parse_sigs.push_back(*sig);
+            state.vals.push_back(sigop.default_value);
+            break;
+          }
+        }
+
+      }
+
+      message_states[state.address] = state;
+    }
+  }
+
+  void UpdateCans(uint64_t sec, const capnp::List<cereal::CanData>::Reader& cans) {
+      int msg_count = cans.size();
+
+      DEBUG("got %d messages\n", msg_count);
+
+      // parse the messages
+      for (int i = 0; i < msg_count; i++) {
+        auto cmsg = cans[i];
+        if (cmsg.getSrc() != bus) {
+          // DEBUG("skip %d: wrong bus\n", cmsg.getAddress());
+          continue;
+        }
+        auto state_it = message_states.find(cmsg.getAddress());
+        if (state_it == message_states.end()) {
+          // DEBUG("skip %d: not specified\n", cmsg.getAddress());
+          continue;
+        }
+
+        if (cmsg.getDat().size() > 8) continue; //shouldnt ever happen
+        uint8_t dat[8] = {0};
+        memcpy(dat, cmsg.getDat().begin(), cmsg.getDat().size());
+
+        uint64_t p = read_u64_be(dat);
+
+        DEBUG("  proc %X: %lx\n", cmsg.getAddress(), p);
+
+        state_it->second.parse(sec, p);
+      }
+  }
+
+  void UpdateValid(uint64_t sec) {
+    can_valid = true;
+    for (auto &kv : message_states) {
+      auto &state = kv.second;
+      if (state.check_threshold > 0 && (sec - state.seen) > state.check_threshold) {
+        if (state.seen > 0) {
+          INFO("%X TIMEOUT\n", state.address);
+        }
+        can_valid = false;
+      }
+    }
+  }
+
+  void update(uint64_t sec, bool wait) {
+    int err;
+
+    // recv from can
+    zmq_msg_t msg;
+    zmq_msg_init(&msg);
+
+    // multiple recv is fine
+    bool first = wait;
+    while (1) {
+      if (first) {
+        err = zmq_msg_recv(&msg, subscriber, 0);
+        first = false;
+      } else {
+        err = zmq_msg_recv(&msg, subscriber, ZMQ_DONTWAIT);
+      }
+      if (err < 0) break;
+
+      // format for board, make copy due to alignment issues, will be freed on out of scope
+      auto amsg = kj::heapArray<capnp::word>((zmq_msg_size(&msg) / sizeof(capnp::word)) + 1);
+      memcpy(amsg.begin(), zmq_msg_data(&msg), zmq_msg_size(&msg));
+
+      // extract the messages
+      capnp::FlatArrayMessageReader cmsg(amsg);
+      cereal::Event::Reader event = cmsg.getRoot<cereal::Event>();
+      
+      auto cans = event.getCan();
+
+      UpdateCans(sec, cans);
+    }
+
+    UpdateValid(sec);
+
+  }
+
+  std::vector<SignalValue> query(uint64_t sec) {
+    std::vector<SignalValue> ret;
+
+    for (auto &kv : message_states) {
+      auto &state = kv.second;
+      if (sec != 0 && state.seen != sec) continue;
+      
+      for (int i=0; i<state.parse_sigs.size(); i++) {
+        const Signal &sig = state.parse_sigs[i];
+        ret.push_back((SignalValue){
+          .address = state.address,
+          .name = sig.name,
+          .value = state.vals[i],
+        });
+      }
+    }
+
+    return ret;
+  }
+
+  bool can_valid = false;
+
+ private:
+  const int bus;
+  // zmq vars
+  void *context = NULL;
+  void *subscriber = NULL;
+
+  const DBC *dbc = NULL;
+  std::unordered_map<uint32_t, MessageState> message_states;
+};
+
+}
+
+void dbc_register(const DBC* dbc) {
+  g_dbc.push_back(dbc);
+}
+
+extern "C" {
+
+void* can_init(int bus, const char* dbc_name,
+              size_t num_message_options, const MessageParseOptions* message_options,
+              size_t num_signal_options, const SignalParseOptions* signal_options) {
+  CANParser* ret = new CANParser(bus, std::string(dbc_name),
+    (message_options ? std::vector<MessageParseOptions>(message_options, message_options+num_message_options)
+      : std::vector<MessageParseOptions>{}),
+    (signal_options ? std::vector<SignalParseOptions>(signal_options, signal_options+num_signal_options)
+      : std::vector<SignalParseOptions>{}));
+  return (void*)ret;
+}
+
+void can_update(void* can, uint64_t sec, bool wait) {
+  CANParser* cp = (CANParser*)can;
+  cp->update(sec, wait);
+}
+
+size_t can_query(void* can, uint64_t sec, bool *out_can_valid, size_t out_values_size, SignalValue* out_values) {
+  CANParser* cp = (CANParser*)can;
+
+  if (out_can_valid) {
+    *out_can_valid = cp->can_valid;
+  }
+
+  const std::vector<SignalValue> values = cp->query(sec);
+  if (out_values) {
+    std::copy(values.begin(), values.begin()+std::min(out_values_size, values.size()), out_values);
+  }
+  return values.size();
+};
+
+}
+
+#ifdef TEST
+
+int main(int argc, char** argv) {
+  CANParser cp(0, "honda_civic_touring_2016_can",
+    std::vector<MessageParseOptions>{
+      // address, check_frequency
+      {0x14a, 100},
+      {0x158, 100},
+      {0x17c, 100},
+      {0x191, 100},
+      {0x1a4, 50},
+      {0x326, 10},
+      {0x1b0, 50},
+      {0x1d0, 50},
+      {0x305, 10},
+      {0x324, 10},
+      {0x405, 3},
+      {0x18f, 0},
+      {0x130, 0},
+      {0x296, 0},
+      {0x30c, 0},
+    },
+    std::vector<SignalParseOptions>{
+      //  sig_name, sig_address, default 
+      {0x158, "XMISSION_SPEED", 0},
+      {0x1d0, "WHEEL_SPEED_FL", 0},
+      {0x1d0, "WHEEL_SPEED_FR", 0},
+      {0x1d0, "WHEEL_SPEED_RL", 0},
+      {0x14a, "STEER_ANGLE", 0},
+      {0x18f, "STEER_TORQUE_SENSOR", 0},
+      {0x191, "GEAR", 0},
+      {0x1b0, "WHEELS_MOVING", 1},
+      {0x405, "DOOR_OPEN_FL", 1},
+      {0x405, "DOOR_OPEN_FR", 1},
+      {0x405, "DOOR_OPEN_RL", 1},
+      {0x405, "DOOR_OPEN_RR", 1},
+      {0x324, "CRUISE_SPEED_PCM", 0},
+      {0x305, "SEATBELT_DRIVER_LAMP", 1},
+      {0x305, "SEATBELT_DRIVER_LATCHED", 0},
+      {0x17c, "BRAKE_PRESSED", 0},
+      {0x130, "CAR_GAS", 0},
+      {0x296, "CRUISE_BUTTONS", 0},
+      {0x1a4, "ESP_DISABLED", 1},
+      {0x30c, "HUD_LEAD", 0},
+      {0x1a4, "USER_BRAKE", 0},
+      {0x18f, "STEER_STATUS", 5},
+      {0x1d0, "WHEEL_SPEED_RR", 0},
+      {0x1b0, "BRAKE_ERROR_1", 1},
+      {0x1b0, "BRAKE_ERROR_2", 1},
+      {0x191, "GEAR_SHIFTER", 0},
+      {0x326, "MAIN_ON", 0},
+      {0x17c, "ACC_STATUS", 0},
+      {0x17c, "PEDAL_GAS", 0},
+      {0x296, "CRUISE_SETTING", 0},
+      {0x326, "LEFT_BLINKER", 0},
+      {0x326, "RIGHT_BLINKER", 0},
+      {0x324, "COUNTER", 0},
+      {0x17c, "ENGINE_RPM", 0},
+    });
+
+
+
+  const std::string log_fn = "dats.bin";
+
+  int log_fd = open(log_fn.c_str(), O_RDONLY, 0);
+  assert(log_fd >= 0);
+
+  off_t log_size = lseek(log_fd, 0, SEEK_END);
+  lseek(log_fd, 0, SEEK_SET);
+
+  void* log_data = mmap(NULL, log_size, PROT_READ, MAP_PRIVATE, log_fd, 0);
+  assert(log_data);
+
+  auto words = kj::arrayPtr((const capnp::word*)log_data, log_size/sizeof(capnp::word));
+  while (words.size() > 0) {
+    capnp::FlatArrayMessageReader reader(words);
+
+    auto evt = reader.getRoot<cereal::Event>();
+    auto cans = evt.getCan();
+
+    cp.UpdateCans(0, cans);
+
+    words = kj::arrayPtr(reader.getEnd(), words.end());
+  }
+
+  munmap(log_data, log_size);
+
+  close(log_fd);
+
+  return 0;
+}
+
+#endif
--- a/selfdrive/can/parser.py
+++ b/selfdrive/can/parser.py
@ -0,0 +1,166 @@
+import os
+import time
+import subprocess
+from collections import defaultdict
+
+from cffi import FFI
+
+can_dir = os.path.dirname(os.path.abspath(__file__))
+libdbc_fn = os.path.join(can_dir, "libdbc.so")
+subprocess.check_output(["make"], cwd=can_dir)
+
+ffi = FFI()
+ffi.cdef("""
+
+typedef struct SignalParseOptions {
+  uint32_t address;
+  const char* name;
+  double default_value;
+} SignalParseOptions;
+
+typedef struct MessageParseOptions {
+  uint32_t address;
+  int check_frequency;
+} MessageParseOptions;
+
+typedef struct SignalValue {
+  uint32_t address;
+  const char* name;
+  double value;
+} SignalValue;
+
+void* can_init(int bus, const char* dbc_name,
+              size_t num_message_options, const MessageParseOptions* message_options,
+              size_t num_signal_options, const SignalParseOptions* signal_options);
+
+void can_update(void* can, uint64_t sec, bool wait);
+
+size_t can_query(void* can, uint64_t sec, bool *out_can_valid, size_t out_values_size, SignalValue* out_values);
+
+""")
+
+libdbc = ffi.dlopen(libdbc_fn)
+
+class CANParser(object):
+  def __init__(self, dbc_name, signals, checks=[], bus=0):
+    self.can_valid = True
+    self.vl = defaultdict(dict)
+
+    sig_names = dict((name, ffi.new("char[]", name)) for name, _, _ in signals)
+
+    signal_options_c = ffi.new("SignalParseOptions[]", [
+      {
+        'address': sig_address,
+        'name': sig_names[sig_name],
+        'default_value': sig_default,
+      } for sig_name, sig_address, sig_default in signals])
+
+    message_options = dict((address, 0) for _, address, _ in signals)
+    message_options.update(dict(checks))
+    
+    message_options_c = ffi.new("MessageParseOptions[]", [
+      {
+        'address': address,
+        'check_frequency': freq,
+      } for address, freq in message_options.iteritems()])
+
+    self.can = libdbc.can_init(bus, dbc_name, len(message_options_c), message_options_c,
+                               len(signal_options_c), signal_options_c)
+
+    self.p_can_valid = ffi.new("bool*")
+
+    value_count = libdbc.can_query(self.can, 0, self.p_can_valid, 0, ffi.NULL)
+    self.can_values = ffi.new("SignalValue[%d]" % value_count)
+    self.update_vl(0)
+    # print "==="
+
+  def update_vl(self, sec):
+
+    can_values_len = libdbc.can_query(self.can, sec, self.p_can_valid, len(self.can_values), self.can_values)
+    assert can_values_len <= len(self.can_values)
+
+    self.can_valid = self.p_can_valid[0]
+
+    # print can_values_len
+    ret = set()
+    for i in xrange(can_values_len):
+      cv = self.can_values[i]
+      address = cv.address
+      # print hex(cv.address), ffi.string(cv.name)
+      self.vl[address][ffi.string(cv.name)] = cv.value
+      ret.add(address)
+    return ret
+
+  def update(self, sec, wait):
+    libdbc.can_update(self.can, sec, wait)
+    return self.update_vl(sec)
+
+if __name__ == "__main__":
+  from common.realtime import sec_since_boot
+
+  radar_messages = range(0x430, 0x43A) + range(0x440, 0x446)
+  # signals = zip(['LONG_DIST'] * 16 + ['NEW_TRACK'] * 16 + ['LAT_DIST'] * 16 +
+  #               ['REL_SPEED'] * 16, radar_messages * 4,
+  #               [255] * 16 + [1] * 16 + [0] * 16 + [0] * 16)
+  # checks = zip(radar_messages, [20]*16)
+
+  # cp = CANParser("acura_ilx_2016_nidec", signals, checks, 1)
+
+  signals = [
+    ("XMISSION_SPEED", 0x158, 0), #sig_name, sig_address, default 
+    ("WHEEL_SPEED_FL", 0x1d0, 0),
+    ("WHEEL_SPEED_FR", 0x1d0, 0),
+    ("WHEEL_SPEED_RL", 0x1d0, 0),
+    ("STEER_ANGLE", 0x14a, 0),
+    ("STEER_TORQUE_SENSOR", 0x18f, 0),
+    ("GEAR", 0x191, 0),
+    ("WHEELS_MOVING", 0x1b0, 1),
+    ("DOOR_OPEN_FL", 0x405, 1),
+    ("DOOR_OPEN_FR", 0x405, 1),
+    ("DOOR_OPEN_RL", 0x405, 1),
+    ("DOOR_OPEN_RR", 0x405, 1),
+    ("CRUISE_SPEED_PCM", 0x324, 0),
+    ("SEATBELT_DRIVER_LAMP", 0x305, 1),
+    ("SEATBELT_DRIVER_LATCHED", 0x305, 0),
+    ("BRAKE_PRESSED", 0x17c, 0),
+    ("CAR_GAS", 0x130, 0),
+    ("CRUISE_BUTTONS", 0x296, 0),
+    ("ESP_DISABLED", 0x1a4, 1),
+    ("HUD_LEAD", 0x30c, 0),
+    ("USER_BRAKE", 0x1a4, 0),
+    ("STEER_STATUS", 0x18f, 5),
+    ("WHEEL_SPEED_RR", 0x1d0, 0),
+    ("BRAKE_ERROR_1", 0x1b0, 1),
+    ("BRAKE_ERROR_2", 0x1b0, 1),
+    ("GEAR_SHIFTER", 0x191, 0),
+    ("MAIN_ON", 0x326, 0),
+    ("ACC_STATUS", 0x17c, 0),
+    ("PEDAL_GAS", 0x17c, 0),
+    ("CRUISE_SETTING", 0x296, 0),
+    ("LEFT_BLINKER", 0x326, 0),
+    ("RIGHT_BLINKER", 0x326, 0),
+    ("COUNTER", 0x324, 0),
+    ("ENGINE_RPM", 0x17C, 0)
+  ]
+  checks = [
+    (0x14a, 100), # address, frequency
+    (0x158, 100),
+    (0x17c, 100),
+    (0x191, 100),
+    (0x1a4, 50),
+    (0x326, 10),
+    (0x1b0, 50),
+    (0x1d0, 50),
+    (0x305, 10),
+    (0x324, 10),
+    (0x405, 3),
+  ]
+
+  cp = CANParser("honda_civic_touring_2016_can", signals, checks, 0)
+  print cp.vl
+
+  while True:
+    cp.update(int(sec_since_boot()*1e9), True)
+    print cp.vl
+    print cp.can_valid
+    time.sleep(0.01)
--- a/selfdrive/can/parser_common.h
+++ b/selfdrive/can/parser_common.h
@ -0,0 +1,63 @@
+#ifndef PARSER_COMMON_H
+#define PARSER_COMMON_H
+
+#include <cstddef>
+#include <cstdint>
+
+#define ARRAYSIZE(x) (sizeof(x)/sizeof(x[0]))
+
+
+
+struct SignalParseOptions {
+  uint32_t address;
+  const char* name;
+  double default_value;
+};
+
+struct MessageParseOptions {
+  uint32_t address;
+  int check_frequency;
+};
+
+struct SignalValue {
+  uint32_t address;
+  const char* name;
+  double value;
+};
+
+
+enum SignalType {
+  DEFAULT,
+  HONDA_CHECKSUM,
+  HONDA_COUNTER,
+};
+
+struct Signal {
+  const char* name;
+  int b1, b2, bo;
+  bool is_signed;
+  double factor, offset;
+  SignalType type;
+};
+
+struct Msg {
+  const char* name;
+  uint32_t address;
+  size_t num_sigs;
+  const Signal *sigs;
+};
+
+struct DBC {
+  const char* name;
+  size_t num_msgs;
+  const Msg *msgs;
+};
+
+void dbc_register(const DBC* dbc);
+
+#define dbc_init(dbc) \
+static void __attribute__((constructor)) do_dbc_init_ ## dbc(void) { \
+  dbc_register(&dbc); \
+}
+
+#endif
--- a/selfdrive/can/process_dbc.py
+++ b/selfdrive/can/process_dbc.py
@ -0,0 +1,32 @@
+#!/usr/bin/env python
+import os
+import sys
+
+import jinja2
+
+import opendbc
+from common.dbc import dbc
+
+if len(sys.argv) != 3:
+  print "usage: %s dbc_path struct_path" % (sys.argv[0],)
+  sys.exit(0)
+
+dbc_fn = sys.argv[1]
+out_fn = sys.argv[2]
+
+template_fn = os.path.join(os.path.dirname(__file__), "dbc_template.cc")
+
+can_dbc = dbc(dbc_fn)
+
+with open(template_fn, "r") as template_f:
+  template = jinja2.Template(template_f.read(), trim_blocks=True, lstrip_blocks=True)
+
+msgs = [(address, msg_name, sorted(msg_sigs, key=lambda s: s.name not in ("COUNTER", "CHECKSUM"))) # process counter and checksums first
+        for address, ((msg_name, _), msg_sigs) in sorted(can_dbc.msgs.iteritems()) if msg_sigs]
+
+checksum_type = "honda" if can_dbc.name.startswith("honda") or can_dbc.name.startswith("acura") else None
+
+parser_code = template.render(dbc=can_dbc, checksum_type=checksum_type, msgs=msgs, len=len)
+
+with open(out_fn, "w") as out_f:
+  out_f.write(parser_code)
--- a/selfdrive/car/init.py
+++ b/selfdrive/car/init.py
@ -0,0 +1,30 @@
+from common.fingerprints import fingerprint
+
+from .honda.interface import CarInterface as HondaInterface
+
+try:
+  from .simulator.interface import CarInterface as SimInterface
+except ImportError:
+  SimInterface = None
+
+try:
+  from .simulator2.interface import CarInterface as Sim2Interface
+except ImportError:
+  Sim2Interface = None
+
+interfaces = {
+  "HONDA CIVIC 2016 TOURING": HondaInterface,
+  "ACURA ILX 2016 ACURAWATCH PLUS": HondaInterface,
+  "HONDA ACCORD 2016 TOURING": HondaInterface,
+  "HONDA CR-V 2016 TOURING": HondaInterface,
+
+  "simulator": SimInterface,
+  "simulator2": Sim2Interface
+}
+
+def get_car(logcan, sendcan=None):
+  candidate, fingerprints = fingerprint(logcan)
+  interface_cls = interfaces[candidate]
+  params = interface_cls.get_params(candidate, fingerprints)
+
+  return interface_cls(params, logcan, sendcan), params
--- a/selfdrive/car/honda/carcontroller.py
+++ b/selfdrive/car/honda/carcontroller.py
@ -1,11 +1,14 @@
 from collections import namedtuple

 import common.numpy_fast as np
+from common.numpy_fast import clip, interp
 from common.realtime import sec_since_boot
+
 from selfdrive.config import CruiseButtons
 from selfdrive.boardd.boardd import can_list_to_can_capnp
 from selfdrive.controls.lib.drive_helpers import rate_limit
-from common.numpy_fast import clip, interp
+from . import hondacan
+

 def actuator_hystereses(final_brake, braking, brake_steady, v_ego, civic):
  # hyst params... TODO: move these to VehicleParams
@ -66,7 +69,6 @@ def process_hud_alert(hud_alert):

  return fcw_display, steer_required, acc_alert

-import selfdrive.car.honda.hondacan as hondacan

 HUDData = namedtuple("HUDData",
                     ["pcm_accel", "v_cruise", "X2", "car", "X4", "X5",
@ -126,7 +128,7 @@ class CarController(object):
    #print chime, alert_id, hud_alert
    fcw_display, steer_required, acc_alert = process_hud_alert(hud_alert)

-    hud = HUDData(int(pcm_accel), int(hud_v_cruise), 0x01, hud_car,
+    hud = HUDData(int(pcm_accel), int(round(hud_v_cruise)), 0x01, hud_car,
                  0xc1, 0x41, hud_lanes + steer_required,
                  int(snd_beep), 0x48, (snd_chime << 5) + fcw_display, acc_alert)

--- a/selfdrive/car/honda/carstate.py
+++ b/selfdrive/car/honda/carstate.py
@ -1,15 +1,22 @@
-import common.numpy_fast as np
+import os
+import time

-import selfdrive.messaging as messaging
+import common.numpy_fast as np
 from common.realtime import sec_since_boot

+import selfdrive.messaging as messaging
+
 from selfdrive.car.honda.can_parser import CANParser
+from selfdrive.can.parser import CANParser as CANParserC
+
+NEW_CAN = os.getenv("OLD_CAN") is None

 def get_can_parser(CP):
  # this function generates lists for signal, messages and initial values
  if CP.carFingerprint == "HONDA CIVIC 2016 TOURING":
    dbc_f = 'honda_civic_touring_2016_can.dbc'
    signals = [
+      # sig_name, sig_address, default
      ("XMISSION_SPEED", 0x158, 0),
      ("WHEEL_SPEED_FL", 0x1d0, 0),
      ("WHEEL_SPEED_FR", 0x1d0, 0),
@ -46,6 +53,7 @@ def get_can_parser(CP):
      ("ENGINE_RPM", 0x17C, 0)
    ]
    checks = [
+      # address, frequency
      (0x14a, 100),
      (0x158, 100),
      (0x17c, 100),
@ -219,7 +227,10 @@ def get_can_parser(CP):
    signals.append(("INTERCEPTOR_GAS", 0x201, 0))
    checks.append((0x201, 50))

-  return CANParser(dbc_f, signals, checks)
+  if NEW_CAN:
+    return CANParserC(os.path.splitext(dbc_f)[0], signals, checks, 0)
+  else:
+    return CANParser(dbc_f, signals, checks)

 class CarState(object):
  def __init__(self, CP, logcan):
@ -256,9 +267,12 @@ class CarState(object):
    # TODO: actually make this work
    self.a_ego = 0.

-  def update(self, can_pub_main):
+  def update(self, can_pub_main=None):
    cp = self.cp
-    cp.update_can(can_pub_main)
+    if NEW_CAN:
+      cp.update(int(sec_since_boot() * 1e9), False)
+    else:
+      cp.update_can(can_pub_main)

    # copy can_valid
    self.can_valid = cp.can_valid
--- a/selfdrive/car/honda/interface.py
+++ b/selfdrive/car/honda/interface.py
@ -1,18 +1,20 @@
 #!/usr/bin/env python
+import os
 import time
 import common.numpy_fast as np

 from selfdrive.config import Conversions as CV
-from selfdrive.car.honda.carstate import CarState
-from selfdrive.car.honda.carcontroller import CarController, AH
+from .carstate import CarState
+from .carcontroller import CarController, AH
 from selfdrive.boardd.boardd import can_capnp_to_can_list

 from cereal import car

-import zmq
 from selfdrive.services import service_list
 import selfdrive.messaging as messaging

+NEW_CAN = os.getenv("OLD_CAN") is None
+
 # Car button codes
 class CruiseButtons:
  RES_ACCEL   = 4
@ -35,7 +37,6 @@ class BP:
  TRIPLE = 2
  REPEATED = 1

-
 class CarInterface(object):
  def __init__(self, CP, logcan, sendcan=None):
    self.logcan = logcan
@ -55,19 +56,65 @@ class CarInterface(object):
    if self.CS.accord:
      self.accord_msg = []

+  @staticmethod
+  def get_params(candidate, fingerprint):
+
+    # pedal
+    brake_only = 0x201 not in fingerprint
+
+    ret = car.CarParams.new_message()
+
+    ret.carName = "honda"
+    ret.radarName = "nidec"
+    ret.carFingerprint = candidate
+
+    ret.enableSteer = True
+    ret.enableBrake = True
+    ret.enableGas = not brake_only
+    ret.enableCruise = brake_only
+    #ret.enableCruise = False
+
+    # TODO: those parameters should be platform dependent
+    ret.wheelBase = 2.67
+    ret.slipFactor = 0.0014
+
+    if candidate == "HONDA CIVIC 2016 TOURING":
+      ret.steerRatio = 13.0
+      ret.steerKp, ret.steerKi = 6.0, 1.4
+    elif candidate == "ACURA ILX 2016 ACURAWATCH PLUS":
+      ret.steerRatio = 15.3
+      if not brake_only:
+        # assuming if we have an interceptor we also have a torque mod
+        ret.steerKp, ret.steerKi = 3.0, 0.7
+      else:
+        ret.steerKp, ret.steerKi = 6.0, 1.4
+    elif candidate == "HONDA ACCORD 2016 TOURING":
+      ret.steerRatio = 15.3
+      ret.steerKp, ret.steerKi = 6.0, 1.4
+    elif candidate == "HONDA CR-V 2016 TOURING":
+      ret.steerRatio = 15.3
+      ret.steerKp, ret.steerKi = 3.0, 0.7
+    else:
+      raise ValueError("unsupported car %s" % candidate)
+
+    return ret
+
  # returns a car.CarState
  def update(self):
    # ******************* do can recv *******************
    can_pub_main = []
    canMonoTimes = []

-    for a in messaging.drain_sock(self.logcan):
-      canMonoTimes.append(a.logMonoTime)
-      can_pub_main.extend(can_capnp_to_can_list(a.can, [0,2]))
-      if self.CS.accord:
-        self.accord_msg.extend(can_capnp_to_can_list(a.can, [9]))
-        self.accord_msg = self.accord_msg[-1:]
-    self.CS.update(can_pub_main)
+    if NEW_CAN:
+      self.CS.update(can_pub_main)
+    else:
+      for a in messaging.drain_sock(self.logcan):
+        canMonoTimes.append(a.logMonoTime)
+        can_pub_main.extend(can_capnp_to_can_list(a.can, [0,0x80]))
+        if self.CS.accord:
+          self.accord_msg.extend(can_capnp_to_can_list(a.can, [9]))
+          self.accord_msg = self.accord_msg[-1:]
+      self.CS.update(can_pub_main)

    # create message
    ret = car.CarState.new_message()
@ -115,6 +162,7 @@ class CarInterface(object):
    # cruise state
    ret.cruiseState.enabled = self.CS.pcm_acc_status != 0
    ret.cruiseState.speed = self.CS.v_cruise_pcm * CV.KPH_TO_MS
+    ret.cruiseState.mainOn = bool(self.CS.main_on)

    # TODO: button presses
    buttonEvents = []
@ -178,7 +226,7 @@ class CarInterface(object):
    if self.CS.steer_error:
      errors.append('steerUnavailable')
    elif self.CS.steer_not_allowed:
-      errors.append('steerTemporarilyUnavailable')
+      errors.append('steerTempUnavailable')
    if self.CS.brake_error:
      errors.append('brakeUnavailable')
    if not self.CS.gear_shifter_valid:
--- a/selfdrive/common/params.c
+++ b/selfdrive/common/params.c
@ -47,14 +47,15 @@ int write_db_value(const char* params_path, const char* key, const char* value,
    goto cleanup;
  }

-  // Move temp into place.
-  result = rename(tmp_path, path);
+  // fsync to force persist the changes.
+  result = fsync(tmp_fd);
  if (result < 0) {
    goto cleanup;
  }

-  // fsync to force persist the changes.
-  result = fsync(tmp_fd);
+  // Move temp into place.
+  result = rename(tmp_path, path);
+
 cleanup:
  // Release lock.
  if (lock_fd >= 0) {
--- a/selfdrive/common/timing.h
+++ b/selfdrive/common/timing.h
@ -4,6 +4,10 @@
 #include <stdint.h>
 #include <time.h>

+#ifdef __APPLE__
+#define CLOCK_BOOTTIME CLOCK_REALTIME
+#endif
+
 static inline uint64_t nanos_since_boot() {
  struct timespec t;
  clock_gettime(CLOCK_BOOTTIME, &t);
--- a/selfdrive/common/util.c
+++ b/selfdrive/common/util.c
@ -12,13 +12,16 @@ void* read_file(const char* path, size_t* out_len) {
  long f_len = ftell(f);
  rewind(f);

-  char* buf = malloc(f_len + 1);
+  char* buf = calloc(f_len + 1, 1);
  assert(buf);
-  memset(buf, 0, f_len + 1);
+
  size_t num_read = fread(buf, f_len, 1, f);
-  assert(num_read == 1);
  fclose(f);

+  if (num_read != 1) {
+    return NULL;
+  }
+
  if (out_len) {
    *out_len = f_len + 1;
  }
--- a/selfdrive/common/util.h
+++ b/selfdrive/common/util.h
@ -19,7 +19,10 @@

 #define ARRAYSIZE(x) (sizeof(x)/sizeof(x[0]))

+// Reads a file into a newly allocated buffer.
+//
 // Returns NULL on failure, otherwise the NULL-terminated file contents.
+// The result must be freed by the caller.
 void* read_file(const char* path, size_t* out_len);


--- a/selfdrive/common/utilpp.h
+++ b/selfdrive/common/utilpp.h
@ -5,6 +5,7 @@
 #include <unistd.h>

 #include <string>
+#include <memory>
 #include <sstream>
 #include <fstream>

--- a/selfdrive/common/version.h
+++ b/selfdrive/common/version.h
@ -1 +1 @@
-#define OPENPILOT_VERSION "0.3.3"
+#define OPENPILOT_VERSION "0.3.4"
--- a/selfdrive/controls/controlsd.py
+++ b/selfdrive/controls/controlsd.py
@ -2,19 +2,22 @@
 import os
 import zmq
 import selfdrive.messaging as messaging
+from copy import copy

 from cereal import car, log

-from selfdrive.swaglog import cloudlog
 from common.numpy_fast import clip
 from common.fingerprints import fingerprint

-from selfdrive.config import Conversions as CV
-from selfdrive.services import service_list
 from common.realtime import sec_since_boot, set_realtime_priority, Ratekeeper
 from common.profiler import Profiler
 from common.params import Params

+from selfdrive.swaglog import cloudlog
+from selfdrive.config import Conversions as CV
+from selfdrive.services import service_list
+from selfdrive.car import get_car
+from selfdrive.controls.lib.planner import Planner
 from selfdrive.controls.lib.drive_helpers import learn_angle_offset

 from selfdrive.controls.lib.longcontrol import LongControl
@ -27,321 +30,352 @@ V_CRUISE_MIN = 8
 V_CRUISE_DELTA = 8
 V_CRUISE_ENABLE_MIN = 40

-def controlsd_thread(gctx, rate=100):  #rate in Hz
-  # *** log ***
-  context = zmq.Context()
-  live100 = messaging.pub_sock(context, service_list['live100'].port)
-  carstate = messaging.pub_sock(context, service_list['carState'].port)
-  carcontrol = messaging.pub_sock(context, service_list['carControl'].port)
+AWARENESS_TIME = 360.      # 6 minutes limit without user touching steering wheels
+AWARENESS_PRE_TIME = 20.   # a first alert is issued 20s before start decelerating the car
+AWARENESS_DECEL = -0.2     # car smoothly decel at .2m/s^2 when user is distracted

-  thermal = messaging.sub_sock(context, service_list['thermal'].port)
-  health = messaging.sub_sock(context, service_list['health'].port)
-  plan_sock = messaging.sub_sock(context, service_list['plan'].port)
+# class Cal
+class Calibration:
+  UNCALIBRATED = 0
+  CALIBRATED = 1
+  INVALID = 2

-  logcan = messaging.sub_sock(context, service_list['can'].port)
+# to be used
+class State():
+  DISABLED = 0
+  ENABLED = 1
+  SOFT_DISABLE = 2

-  # connects to can
-  CP = fingerprint(logcan)
+class Controls(object):
+  def __init__(self, gctx, rate=100):
+    self.rate = rate

-  # import the car from the fingerprint
-  cloudlog.info("controlsd is importing %s", CP.carName)
-  exec('from selfdrive.car.'+CP.carName+'.interface import CarInterface')
+    # *** log ***
+    context = zmq.Context()

-  sendcan = messaging.pub_sock(context, service_list['sendcan'].port)
-  CI = CarInterface(CP, logcan, sendcan)
+    # pub
+    self.live100 = messaging.pub_sock(context, service_list['live100'].port)
+    self.carstate = messaging.pub_sock(context, service_list['carState'].port)
+    self.carcontrol = messaging.pub_sock(context, service_list['carControl'].port)
+    sendcan = messaging.pub_sock(context, service_list['sendcan'].port)
+ 
+    # sub
+    self.thermal = messaging.sub_sock(context, service_list['thermal'].port)
+    self.health = messaging.sub_sock(context, service_list['health'].port)
+    logcan = messaging.sub_sock(context, service_list['can'].port)
+    self.cal = messaging.sub_sock(context, service_list['liveCalibration'].port)
+    
+    self.CC = car.CarControl.new_message()
+    self.CI, self.CP = get_car(logcan, sendcan)
+    self.PL = Planner(self.CP)
+    self.AM = AlertManager()
+    self.LoC = LongControl()
+    self.LaC = LatControl()
+  
+    # write CarParams
+    params = Params()
+    params.put("CarParams", self.CP.to_bytes())
+  
+    # fake plan
+    self.plan_ts = 0
+    self.plan = log.Plan.new_message()
+    self.plan.lateralValid = False
+    self.plan.longitudinalValid = False
+  
+    # controls enabled state
+    self.enabled = False
+    self.last_enable_request = 0
+  
+    # learned angle offset
+    self.angle_offset = 0
+  
+    # rear view camera state
+    self.rear_view_toggle = False
+    self.rear_view_allowed = bool(params.get("IsRearViewMirror"))
+  
+    self.v_cruise_kph = 255
+  
+    # 0.0 - 1.0
+    self.awareness_status = 1.0
+  
+    self.soft_disable_timer = None
+  
+    self.overtemp = False
+    self.free_space = 1.0
+    self.cal_status = Calibration.UNCALIBRATED
+    self.cal_perc = 0
+  
+    self.rk = Ratekeeper(self.rate, print_delay_threshold=2./1000)
+ 
+  def data_sample(self):
+    self.prof = Profiler()
+    self.cur_time = sec_since_boot()
+    # first read can and compute car states
+    self.CS = self.CI.update()

-  # write CarParams
-  params = Params()
-  params.put("CarParams", CP.to_bytes())
-
-  AM = AlertManager()
-
-  LoC = LongControl()
-  LaC = LatControl()
-
-  # fake plan
-  plan = log.Plan.new_message()
-  plan.lateralValid = False
-  plan.longitudinalValid = False
-  last_plan_time = 0
-
-  # controls enabled state
-  enabled = False
-  last_enable_request = 0
-
-  # learned angle offset
-  angle_offset = 0
-
-  # rear view camera state
-  rear_view_toggle = False
-  rear_view_allowed = bool(params.get("IsRearViewMirror"))
-
-  v_cruise_kph = 255
-
-  # 0.0 - 1.0
-  awareness_status = 0.0
-
-  soft_disable_timer = None
-
-  # Is cpu temp too high to enable?
-  overtemp = False
-  free_space = 1.0
-
-  # start the loop
-  set_realtime_priority(2)
-
-  rk = Ratekeeper(rate, print_delay_threshold=2./1000)
-  while 1:
-    cur_time = sec_since_boot()
-    prof = Profiler()
-
-    # read CAN
-    CS = CI.update()
-
-    prof.checkpoint("CarInterface")
-
-    # broadcast carState
-    cs_send = messaging.new_message()
-    cs_send.init('carState')
-    cs_send.carState = CS    # copy?
-    carstate.send(cs_send.to_bytes())
-
-    prof.checkpoint("CarState")
-
-    # did it request to enable?
-    enable_request, enable_condition = False, False
-
-    if enabled:
-      # gives the user 6 minutes
-      awareness_status -= 1.0/(100*60*6)
-      if awareness_status <= 0.:
-        AM.add("driverDistracted", enabled)
-
-    # reset awareness status on steering
-    if CS.steeringPressed:
-      awareness_status = 1.0
-
-    # handle button presses
-    for b in CS.buttonEvents:
-      print b
-
-      # reset awareness on any user action
-      awareness_status = 1.0
-
-      # button presses for rear view
-      if b.type == "leftBlinker" or b.type == "rightBlinker":
-        if b.pressed and rear_view_allowed:
-          rear_view_toggle = True
-        else:
-          rear_view_toggle = False
-
-      if b.type == "altButton1" and b.pressed:
-        rear_view_toggle = not rear_view_toggle
-
-      if not CP.enableCruise and enabled and not b.pressed:
-        if b.type == "accelCruise":
-          v_cruise_kph = v_cruise_kph - (v_cruise_kph % V_CRUISE_DELTA) + V_CRUISE_DELTA
-        elif b.type == "decelCruise":
-          v_cruise_kph = v_cruise_kph - (v_cruise_kph % V_CRUISE_DELTA) - V_CRUISE_DELTA
-        v_cruise_kph = clip(v_cruise_kph, V_CRUISE_MIN, V_CRUISE_MAX)
-
-      if not enabled and b.type in ["accelCruise", "decelCruise"] and not b.pressed:
-        enable_request = True
-
-      # do disable on button down
-      if b.type == "cancel" and b.pressed:
-        AM.add("disable", enabled)
-
-    prof.checkpoint("Buttons")
-
-    # *** health checking logic ***
-    hh = messaging.recv_sock(health)
-    if hh is not None:
-      # if the board isn't allowing controls but somehow we are enabled!
-      if not hh.health.controlsAllowed and enabled:
-        AM.add("controlsMismatch", enabled)
+    self.prof.checkpoint("CarInterface")

    # *** thermal checking logic ***
-
    # thermal data, checked every second
-    td = messaging.recv_sock(thermal)
+    td = messaging.recv_sock(self.thermal)
    if td is not None:
      # Check temperature.
-      overtemp = any(
+      self.overtemp = any(
          t > 950
          for t in (td.thermal.cpu0, td.thermal.cpu1, td.thermal.cpu2,
                    td.thermal.cpu3, td.thermal.mem, td.thermal.gpu))
      # under 15% of space free
-      free_space = td.thermal.freeSpace
+      self.free_space = td.thermal.freeSpace

-    prof.checkpoint("Health")
+    # read calibration status
+    cal = messaging.recv_sock(self.cal)
+    if cal is not None:
+      self.cal_status = cal.liveCalibration.calStatus
+      self.cal_perc = cal.liveCalibration.calPerc
+    
+
+  def state_transition(self):
+    pass # for now
+
+  def state_control(self):
+    
+    # did it request to enable?
+    enable_request, enable_condition = False, False
+
+    # reset awareness status on steering
+    if self.CS.steeringPressed or not self.enabled:
+      self.awareness_status = 1.0
+    elif self.enabled:
+      # gives the user 6 minutes
+      self.awareness_status -= 1.0/(self.rate * AWARENESS_TIME)
+      if self.awareness_status <= 0.:
+        self.AM.add("driverDistracted", self.enabled)
+      elif self.awareness_status <= AWARENESS_PRE_TIME / AWARENESS_TIME and \
+           self.awareness_status >= (AWARENESS_PRE_TIME - 0.1) / AWARENESS_TIME:
+        self.AM.add("preDriverDistracted", self.enabled)
+
+    # handle button presses
+    for b in self.CS.buttonEvents:
+      print b
+
+      # button presses for rear view
+      if b.type == "leftBlinker" or b.type == "rightBlinker":
+        if b.pressed and self.rear_view_allowed:
+          self.rear_view_toggle = True
+        else:
+          self.rear_view_toggle = False
+
+      if b.type == "altButton1" and b.pressed:
+        self.rear_view_toggle = not self.rear_view_toggle
+
+      if not self.CP.enableCruise and self.enabled and not b.pressed:
+        if b.type == "accelCruise":
+          self.v_cruise_kph -= (self.v_cruise_kph % V_CRUISE_DELTA) - V_CRUISE_DELTA
+        elif b.type == "decelCruise":
+          self.v_cruise_kph -= (self.v_cruise_kph % V_CRUISE_DELTA) + V_CRUISE_DELTA
+        self.v_cruise_kph = clip(self.v_cruise_kph, V_CRUISE_MIN, V_CRUISE_MAX)
+
+      if not self.enabled and b.type in ["accelCruise", "decelCruise"] and not b.pressed:
+        enable_request = True
+
+      # do disable on button down
+      if b.type == "cancel" and b.pressed:
+        self.AM.add("disable", self.enabled)
+
+    self.prof.checkpoint("Buttons")
+    
+    # *** health checking logic ***
+    hh = messaging.recv_sock(self.health)
+    if hh is not None:
+      # if the board isn't allowing controls but somehow we are enabled!
+      # TODO: this should be in state transition with a function follower logic
+      if not hh.health.controlsAllowed and self.enabled:
+        self.AM.add("controlsMismatch", self.enabled)

    # disable if the pedals are pressed while engaged, this is a user disable
-    if enabled:
-      if CS.gasPressed or CS.brakePressed:
-        AM.add("disable", enabled)
+    if self.enabled:
+      if self.CS.gasPressed or self.CS.brakePressed or not self.CS.cruiseState.mainOn:
+        self.AM.add("disable", self.enabled)

-      # how did we get into this state?
-      if CP.enableCruise and not CS.cruiseState.enabled:
-        AM.add("cruiseDisabled", enabled)
+      # it can happen that car cruise disables while comma system is enabled: need to
+      # keep braking if needed or if the speed is very low
+      # TODO: for the Acura, cancellation below 25mph is normal. Issue a non loud alert
+      if self.CP.enableCruise and not self.CS.cruiseState.enabled and \
+         (self.CC.brake <= 0. or self.CS.vEgo < 0.3):
+        self.AM.add("cruiseDisabled", self.enabled)

    if enable_request:
      # check for pressed pedals
-      if CS.gasPressed or CS.brakePressed:
-        AM.add("pedalPressed", enabled)
+      if self.CS.gasPressed or self.CS.brakePressed:
+        self.AM.add("pedalPressed", self.enabled)
        enable_request = False
      else:
-        print "enabled pressed at", cur_time
-        last_enable_request = cur_time
+        print "enabled pressed at", self.cur_time
+        self.last_enable_request = self.cur_time

      # don't engage with less than 15% free
-      if free_space < 0.15:
-        AM.add("outOfSpace", enabled)
+      if self.free_space < 0.15:
+        self.AM.add("outOfSpace", self.enabled)
        enable_request = False

-    if CP.enableCruise:
-      enable_condition = ((cur_time - last_enable_request) < 0.2) and CS.cruiseState.enabled
+    if self.CP.enableCruise:
+      enable_condition = ((self.cur_time - self.last_enable_request) < 0.2) and self.CS.cruiseState.enabled
    else:
      enable_condition = enable_request

-    if CP.enableCruise and CS.cruiseState.enabled:
-      v_cruise_kph = CS.cruiseState.speed * CV.MS_TO_KPH
+    if self.CP.enableCruise and self.CS.cruiseState.enabled:
+      self.v_cruise_kph = self.CS.cruiseState.speed * CV.MS_TO_KPH

-    prof.checkpoint("AdaptiveCruise")
+    self.prof.checkpoint("AdaptiveCruise")

    # *** what's the plan ***
-    new_plan = messaging.recv_sock(plan_sock)
-    if new_plan is not None:
-      plan = new_plan.plan
-      plan = plan.as_builder()  # plan can change in controls
-      last_plan_time = cur_time
+    plan_packet = self.PL.update(self.CS, self.LoC)
+    self.plan = plan_packet.plan
+    self.plan_ts = plan_packet.logMonoTime

-    # check plan for timeout
-    if cur_time - last_plan_time > 0.5:
-      plan.lateralValid = False
-      plan.longitudinalValid = False
+    # if user is not responsive to awareness alerts, then start a smooth deceleration
+    if self.awareness_status < -0.:
+      self.plan.aTargetMax = min(self.plan.aTargetMax, AWARENESS_DECEL)
+      self.plan.aTargetMin = min(self.plan.aTargetMin, self.plan.aTargetMax)

-    # gives 18 seconds before decel begins (w 6 minute timeout)
-    if awareness_status < -0.05:
-      plan.aTargetMax = min(plan.aTargetMax, -0.2)
-      plan.aTargetMin = min(plan.aTargetMin, plan.aTargetMax)
-
-    if enable_request or enable_condition or enabled:
+    if enable_request or enable_condition or self.enabled:
      # add all alerts from car
-      for alert in CS.errors:
-        AM.add(alert, enabled)
+      for alert in self.CS.errors:
+        self.AM.add(alert, self.enabled)

-      if not plan.longitudinalValid:
-        AM.add("radarCommIssue", enabled)
+      if not self.plan.longitudinalValid:
+        self.AM.add("radarCommIssue", self.enabled)

-      if not plan.lateralValid:
+      if self.cal_status != Calibration.CALIBRATED:
+        if self.cal_status == Calibration.UNCALIBRATED:
+          self.AM.add("calibrationInProgress", self.enabled, str(self.cal_perc) + '%')
+        else:
+          self.AM.add("calibrationInvalid", self.enabled)
+
+      if not self.plan.lateralValid:
        # If the model is not broadcasting, assume that it is because
        # the user has uploaded insufficient data for calibration.
        # Other cases that would trigger this are rare and unactionable by the user.
-        AM.add("dataNeeded", enabled)
+        self.AM.add("dataNeeded", self.enabled)

-      if overtemp:
-        AM.add("overheat", enabled)
+      if self.overtemp:
+        self.AM.add("overheat", self.enabled)

-    # *** angle offset learning ***
-    if rk.frame % 5 == 2 and plan.lateralValid:
-      # *** run this at 20hz again ***
-      angle_offset = learn_angle_offset(enabled, CS.vEgo, angle_offset, plan.dPoly, LaC.y_des, CS.steeringPressed)
+    
+    # *** angle offset learning *** 
+    if self.rk.frame % 5 == 2 and self.plan.lateralValid: 
+      # *** run this at 20hz again *** 
+      self.angle_offset = learn_angle_offset(self.enabled, self.CS.vEgo, self.angle_offset, 
+                                             self.plan.dPoly, self.LaC.y_des, self.CS.steeringPressed) 

-    # *** gas/brake PID loop ***
-    final_gas, final_brake = LoC.update(enabled, CS.vEgo, v_cruise_kph,
-                                        plan.vTarget,
-                                        [plan.aTargetMin, plan.aTargetMax],
-                                        plan.jerkFactor, CP)
+    # *** gas/brake PID loop *** 
+    final_gas, final_brake = self.LoC.update(self.enabled, self.CS.vEgo, self.v_cruise_kph, 
+                                        self.plan.vTarget, 
+                                        [self.plan.aTargetMin, self.plan.aTargetMax], 
+                                        self.plan.jerkFactor, self.CP) 

-    # *** steering PID loop ***
-    final_steer, sat_flag = LaC.update(enabled, CS.vEgo, CS.steeringAngle, CS.steeringPressed, plan.dPoly, angle_offset, CP)
+    # *** steering PID loop *** 
+    final_steer, sat_flag = self.LaC.update(self.enabled, self.CS.vEgo, self.CS.steeringAngle, 
+                                            self.CS.steeringPressed, self.plan.dPoly, self.angle_offset, self.CP) 
+ 
+    self.prof.checkpoint("PID") 
+    
+        # ***** handle alerts ****
+    # send FCW alert if triggered by planner
+    if self.plan.fcw:
+      self.AM.add("fcw", self.enabled)

-    prof.checkpoint("PID")
-
-    # ***** handle alerts ****
    # send a "steering required alert" if saturation count has reached the limit
    if sat_flag:
-      AM.add("steerSaturated", enabled)
+      self.AM.add("steerSaturated", self.enabled)

-    if enabled and AM.alertShouldDisable():
+    if self.enabled and self.AM.alertShouldDisable():
      print "DISABLING IMMEDIATELY ON ALERT"
-      enabled = False
+      self.enabled = False

-    if enabled and AM.alertShouldSoftDisable():
-      if soft_disable_timer is None:
-        soft_disable_timer = 3 * rate
-      elif soft_disable_timer == 0:
+    if self.enabled and self.AM.alertShouldSoftDisable():
+      if self.soft_disable_timer is None:
+        self.soft_disable_timer = 3 * self.rate
+      elif self.soft_disable_timer == 0:
        print "SOFT DISABLING ON ALERT"
-        enabled = False
+        self.enabled = False
      else:
-        soft_disable_timer -= 1
+        self.soft_disable_timer -= 1
    else:
-      soft_disable_timer = None
+      self.soft_disable_timer = None

-    if enable_condition and not enabled and not AM.alertPresent():
+    if enable_condition and not self.enabled and not self.AM.alertPresent():
      print "*** enabling controls"

      # beep for enabling
-      AM.add("enable", enabled)
+      self.AM.add("enable", self.enabled)

      # enable both lateral and longitudinal controls
-      enabled = True
+      self.enabled = True

      # on activation, let's always set v_cruise from where we are, even if PCM ACC is active
-      v_cruise_kph = int(round(max(CS.vEgo * CV.MS_TO_KPH, V_CRUISE_ENABLE_MIN)))
+      self.v_cruise_kph = int(round(max(self.CS.vEgo * CV.MS_TO_KPH, V_CRUISE_ENABLE_MIN)))

      # 6 minutes driver you're on
-      awareness_status = 1.0
+      self.awareness_status = 1.0

      # reset the PID loops
-      LaC.reset()
+      self.LaC.reset()
      # start long control at actual speed
-      LoC.reset(v_pid = CS.vEgo)
+      self.LoC.reset(v_pid = self.CS.vEgo)

    # *** push the alerts to current ***
-    alert_text_1, alert_text_2, visual_alert, audible_alert = AM.process_alerts(cur_time)
-
+    # TODO: remove output, store them inside AM class instead
+    self.alert_text_1, self.alert_text_2, self.visual_alert, self.audible_alert = self.AM.process_alerts(self.cur_time)
+ 
    # ***** control the car *****
-    CC = car.CarControl.new_message()
+    self.CC.enabled = self.enabled

-    CC.enabled = enabled
+    self.CC.gas = float(final_gas)
+    self.CC.brake = float(final_brake)
+    self.CC.steeringTorque = float(final_steer)

-    CC.gas = float(final_gas)
-    CC.brake = float(final_brake)
-    CC.steeringTorque = float(final_steer)
-
-    CC.cruiseControl.override = True
-    CC.cruiseControl.cancel = bool((not CP.enableCruise) or (not enabled and CS.cruiseState.enabled))    # always cancel if we have an interceptor
+    self.CC.cruiseControl.override = True
+    # always cancel if we have an interceptor
+    self.CC.cruiseControl.cancel = bool(not self.CP.enableCruise or 
+                                        (not self.enabled and self.CS.cruiseState.enabled))

    # brake discount removes a sharp nonlinearity
    brake_discount = (1.0 - clip(final_brake*3., 0.0, 1.0))
-    CC.cruiseControl.speedOverride = float(max(0.0, ((LoC.v_pid - .5) * brake_discount)) if CP.enableCruise else 0.0)
+    self.CC.cruiseControl.speedOverride = float(max(0.0, ((self.LoC.v_pid - .5) * brake_discount)) if self.CP.enableCruise else 0.0)

    #CC.cruiseControl.accelOverride = float(AC.a_pcm)
    # TODO: fix this
-    CC.cruiseControl.accelOverride = float(1.0)
+    self.CC.cruiseControl.accelOverride = float(1.0)

-    CC.hudControl.setSpeed = float(v_cruise_kph * CV.KPH_TO_MS)
-    CC.hudControl.speedVisible = enabled
-    CC.hudControl.lanesVisible = enabled
-    CC.hudControl.leadVisible = plan.hasLead
-
-    CC.hudControl.visualAlert = visual_alert
-    CC.hudControl.audibleAlert = audible_alert
+    self.CC.hudControl.setSpeed = float(self.v_cruise_kph * CV.KPH_TO_MS)
+    self.CC.hudControl.speedVisible = self.enabled
+    self.CC.hudControl.lanesVisible = self.enabled
+    self.CC.hudControl.leadVisible = self.plan.hasLead
+    self.CC.hudControl.visualAlert = self.visual_alert
+    self.CC.hudControl.audibleAlert = self.audible_alert

+    # TODO: remove it from here and put it in state_transition
    # this alert will apply next controls cycle
-    if not CI.apply(CC):
-      AM.add("controlsFailed", enabled)
+    if not self.CI.apply(self.CC):
+      self.AM.add("controlsFailed", self.enabled)

-    # broadcast carControl
+  def data_send(self):
+    
+    # broadcast carControl first
    cc_send = messaging.new_message()
    cc_send.init('carControl')
-    cc_send.carControl = CC    # copy?
-    carcontrol.send(cc_send.to_bytes())
+    cc_send.carControl = copy(self.CC)
+    self.carcontrol.send(cc_send.to_bytes())

-    prof.checkpoint("CarControl")
+    self.prof.checkpoint("CarControl")

+    # broadcast carState
+    cs_send = messaging.new_message()
+    cs_send.init('carState')
+    cs_send.carState = copy(self.CS)
+    self.carstate.send(cs_send.to_bytes())
+    
    # ***** publish state to logger *****

    # publish controls state at 100Hz
@ -349,56 +383,72 @@ def controlsd_thread(gctx, rate=100):  #rate in Hz
    dat.init('live100')

    # show rear view camera on phone if in reverse gear or when button is pressed
-    dat.live100.rearViewCam = ('reverseGear' in CS.errors and rear_view_allowed) or rear_view_toggle
-    dat.live100.alertText1 = alert_text_1
-    dat.live100.alertText2 = alert_text_2
-    dat.live100.awarenessStatus = max(awareness_status, 0.0) if enabled else 0.0
+    dat.live100.rearViewCam = ('reverseGear' in self.CS.errors and self.rear_view_allowed) or self.rear_view_toggle
+    dat.live100.alertText1 = self.alert_text_1
+    dat.live100.alertText2 = self.alert_text_2
+    dat.live100.awarenessStatus = max(self.awareness_status, 0.0) if self.enabled else 0.0

    # what packets were used to process
-    dat.live100.canMonoTimes = list(CS.canMonoTimes)
+    dat.live100.canMonoTimes = list(self.CS.canMonoTimes)
+    dat.live100.planMonoTime = self.plan_ts

    # if controls is enabled
-    dat.live100.enabled = enabled
+    dat.live100.enabled = self.enabled

    # car state
-    dat.live100.vEgo = CS.vEgo
-    dat.live100.angleSteers = CS.steeringAngle
-    dat.live100.steerOverride = CS.steeringPressed
+    dat.live100.vEgo = self.CS.vEgo
+    dat.live100.angleSteers = self.CS.steeringAngle
+    dat.live100.steerOverride = self.CS.steeringPressed

    # longitudinal control state
-    dat.live100.vPid = float(LoC.v_pid)
-    dat.live100.vCruise = float(v_cruise_kph)
-    dat.live100.upAccelCmd = float(LoC.Up_accel_cmd)
-    dat.live100.uiAccelCmd = float(LoC.Ui_accel_cmd)
+    dat.live100.vPid = float(self.LoC.v_pid)
+    dat.live100.vCruise = float(self.v_cruise_kph)
+    dat.live100.upAccelCmd = float(self.LoC.Up_accel_cmd)
+    dat.live100.uiAccelCmd = float(self.LoC.Ui_accel_cmd)

    # lateral control state
-    dat.live100.yActual = float(LaC.y_actual)
-    dat.live100.yDes = float(LaC.y_des)
-    dat.live100.upSteer = float(LaC.Up_steer)
-    dat.live100.uiSteer = float(LaC.Ui_steer)
+    dat.live100.yActual = float(self.LaC.y_actual)
+    dat.live100.yDes = float(self.LaC.y_des)
+    dat.live100.upSteer = float(self.LaC.Up_steer)
+    dat.live100.uiSteer = float(self.LaC.Ui_steer)

    # processed radar state, should add a_pcm?
-    dat.live100.vTargetLead = float(plan.vTarget)
-    dat.live100.aTargetMin = float(plan.aTargetMin)
-    dat.live100.aTargetMax = float(plan.aTargetMax)
-    dat.live100.jerkFactor = float(plan.jerkFactor)
+    dat.live100.vTargetLead = float(self.plan.vTarget)
+    dat.live100.aTargetMin = float(self.plan.aTargetMin)
+    dat.live100.aTargetMax = float(self.plan.aTargetMax)
+    dat.live100.jerkFactor = float(self.plan.jerkFactor)

    # log learned angle offset
-    dat.live100.angleOffset = float(angle_offset)
+    dat.live100.angleOffset = float(self.angle_offset)

    # lag
-    dat.live100.cumLagMs = -rk.remaining*1000.
+    dat.live100.cumLagMs = -self.rk.remaining*1000.

-    live100.send(dat.to_bytes())
+    self.live100.send(dat.to_bytes())

-    prof.checkpoint("Live100")
+    self.prof.checkpoint("Live100")

+  def wait(self):
    # *** run loop at fixed rate ***
-    if rk.keep_time():
-      prof.display()
+    if self.rk.keep_time():
+      self.prof.display()
+
+
+def controlsd_thread(gctx, rate=100):
+  # start the loop
+  set_realtime_priority(2)
+  CTRLS = Controls(gctx, rate)
+  while 1:
+    CTRLS.data_sample()
+    CTRLS.state_transition()
+    CTRLS.state_control()
+    CTRLS.data_send()
+    CTRLS.wait()
+

 def main(gctx=None):
  controlsd_thread(gctx, 100)

 if __name__ == "__main__":
  main()
+ 
--- a/selfdrive/controls/lib/adaptivecruise.py
+++ b/selfdrive/controls/lib/adaptivecruise.py
@ -214,28 +214,6 @@ def calc_jerk_factor(d_lead, v_rel):
  return jerk_factor


-def calc_ttc(d_rel, v_rel, a_rel, v_lead):
-  # this function returns the time to collision (ttc), assuming that a_rel will stay constant
-  # TODO: Review these assumptions.
-  # change sign to rel quantities as it's going to be easier for calculations
-  v_rel = -v_rel
-  a_rel = -a_rel
-
-  # assuming that closing gap a_rel comes from lead vehicle decel, then limit a_rel so that v_lead will get to zero in no sooner than t_decel
-  # this helps overweighting a_rel when v_lead is close to zero.
-  t_decel = 2.
-  a_rel = min(a_rel, v_lead/t_decel)
-
-  delta = v_rel**2 + 2 * d_rel * a_rel
-  # assign an arbitrary high ttc value if there is no solution to ttc
-  if delta < 0.1:
-    ttc = 5.
-  elif math.sqrt(delta) + v_rel < 0.1:
-    ttc = 5.
-  else:
-    ttc = 2 * d_rel / (math.sqrt(delta) + v_rel)
-  return ttc
-

 MAX_SPEED_POSSIBLE = 55.

@ -299,18 +277,14 @@ def compute_speed_with_leads(v_ego, angle_steers, v_pid, l1, l2, CP):


 class AdaptiveCruise(object):
-  def __init__(self, live20):
-    self.live20 = live20
+  def __init__(self):
    self.last_cal = 0.
    self.l1, self.l2 = None, None
-    self.logMonoTime = 0
    self.dead = True
-  def update(self, cur_time, v_ego, angle_steers, v_pid, CP):
-    l20 = messaging.recv_sock(self.live20)
+  def update(self, cur_time, v_ego, angle_steers, v_pid, CP, l20):
    if l20 is not None:
      self.l1 = l20.live20.leadOne
      self.l2 = l20.live20.leadTwo
-      self.logMonoTime = l20.logMonoTime

      # TODO: no longer has anything to do with calibration
      self.last_cal = cur_time
--- a/selfdrive/controls/lib/alertmanager.py
+++ b/selfdrive/controls/lib/alertmanager.py
@ -1,13 +1,14 @@
 from cereal import car
 from selfdrive.swaglog import cloudlog
+import copy

 class ET:
  ENABLE = 0
  NO_ENTRY = 1
  WARNING = 2
-  SOFT_DISABLE = 3
-  IMMEDIATE_DISABLE = 4
-  USER_DISABLE = 5
+  USER_DISABLE = 3
+  SOFT_DISABLE = 4
+  IMMEDIATE_DISABLE = 5

 class alert(object):
  def __init__(self, alert_text_1, alert_text_2, alert_type, visual_alert, audible_alert, duration_sound, duration_hud_alert, duration_text):
@ -34,33 +35,37 @@ class alert(object):

 class AlertManager(object):
  alerts = {
-    "enable":             alert("", "",                                            ET.ENABLE,       None,               "beepSingle", .2, 0., 0.),
-    "disable":            alert("", "",                                            ET.USER_DISABLE, None,               "beepSingle", .2, 0., 0.),
-    "pedalPressed":       alert("Comma Unavailable",        "Pedal Pressed",       ET.NO_ENTRY,     "brakePressed",     "chimeDouble", .4, 2., 3.),
-    "driverDistracted":   alert("Take Control to Regain Speed", "User Distracted", ET.WARNING,      "steerRequired", "chimeRepeated", 1., 1., 1.),
-    "steerSaturated":     alert("Take Control", "Steering Limit Reached",         ET.WARNING,      "steerRequired", "chimeSingle", 1., 2., 3.),
-    "overheat":           alert("Take Control Immediately", "System Overheated",   ET.SOFT_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
-    "controlsMismatch":   alert("Take Control Immediately", "Controls Mismatch",   ET.IMMEDIATE_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
-    "radarCommIssue":     alert("Take Control Immediately", "Radar Comm Issue",    ET.IMMEDIATE_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
-    "modelCommIssue":     alert("Take Control Immediately", "Model Comm Issue",    ET.IMMEDIATE_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
-    "controlsFailed":     alert("Take Control Immediately", "Controls Failed",     ET.IMMEDIATE_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
+    "enable":                alert("", "",                                                    ET.ENABLE,       None,               "beepSingle", .2, 0., 0.),
+    "disable":               alert("", "",                                                    ET.USER_DISABLE, None,               "beepSingle", .2, 0., 0.),
+    "pedalPressed":          alert("Comma Unavailable",        "Pedal Pressed",               ET.NO_ENTRY,     "brakePressed",     "chimeDouble", .4, 2., 3.),
+    "preDriverDistracted":   alert("Take Control ",            "User Distracted",             ET.WARNING,      "steerRequired",    "chimeDouble", .4, 2., 3.),
+    "driverDistracted":      alert("Take Control to Regain Speed", "User Distracted",         ET.WARNING,      "steerRequired", "chimeRepeated", .5, .5, .5),
+    "steerSaturated":        alert("Take Control",             "Turn Exceeds Limit",          ET.WARNING,      "steerRequired", "chimeSingle", 1., 2., 3.),
+    "overheat":              alert("Take Control Immediately", "System Overheated",           ET.SOFT_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
+    "controlsMismatch":      alert("Take Control Immediately", "Controls Mismatch",           ET.IMMEDIATE_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
+    "radarCommIssue":        alert("Take Control Immediately", "Radar Error: Restart the Car",ET.IMMEDIATE_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
+    "calibrationInvalid":    alert("Take Control Immediately", "Calibration Invalid: Reposition Neo and Recalibrate", ET.SOFT_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
+    "calibrationInProgress": alert("Take Control Immediately", "Calibration in Progress: ",     ET.SOFT_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
+    "modelCommIssue":        alert("Take Control Immediately", "Model Error: Restart the Car",ET.IMMEDIATE_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
+    "controlsFailed":        alert("Take Control Immediately", "Controls Failed",             ET.IMMEDIATE_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
+    "fcw":                   alert("", "",                                                    ET.WARNING,           None,            None,            .1, .1, .1),
    # car errors
-    "commIssue":          alert("Take Control Immediately","Communication Issues", ET.IMMEDIATE_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
-    "steerUnavailable":   alert("Take Control Immediately","Steer Error",          ET.IMMEDIATE_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
-    "steerTemporarilyUnavailable": alert("Take Control", "Steer Temporarily Unavailable",  ET.WARNING,   "steerRequired", "chimeRepeated", 1., 2., 3.),
-    "brakeUnavailable":   alert("Take Control Immediately","Brake Error",          ET.IMMEDIATE_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
-    "gasUnavailable":     alert("Take Control Immediately","Gas Error",            ET.IMMEDIATE_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
-    "wrongGear":          alert("Take Control Immediately","Gear not D",           ET.SOFT_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
-    "reverseGear":        alert("Take Control Immediately","Car in Reverse",       ET.IMMEDIATE_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
-    "doorOpen":           alert("Take Control Immediately","Door Open",            ET.SOFT_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
-    "seatbeltNotLatched": alert("Take Control Immediately","Seatbelt Unlatched",   ET.SOFT_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
-    "espDisabled":        alert("Take Control Immediately","ESP Off",              ET.SOFT_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
-    "cruiseDisabled":     alert("Take Control Immediately","Cruise Is Off",        ET.IMMEDIATE_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
-    "wrongCarMode":       alert("Comma Unavailable","Main Switch Off",             ET.NO_ENTRY,     None, "chimeDouble", .4, 0., 3.),
-    "outOfSpace":         alert("Comma Unavailable","Out of Space",                ET.NO_ENTRY,     None, "chimeDouble", .4, 0., 3.),
-    "dataNeeded":         alert("Comma Unavailable","Data needed for calibration. Upload drive, try again", ET.NO_ENTRY, None, "chimeDouble", .4, 0., 3.),
-    "ethicalDilemma":     alert("Take Control Immediately","Ethical Dilemma Detected", ET.IMMEDIATE_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
-    "startup":            alert("Always Keep Hands on Wheel","Be Ready to Take Over Any Time", ET.NO_ENTRY, None, None, 0., 0., 15.),
+    "commIssue":             alert("Take Control Immediately","CAN Error: Restart the Car",   ET.IMMEDIATE_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
+    "steerUnavailable":      alert("Take Control Immediately","Steer Error: Restart the Car", ET.IMMEDIATE_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
+    "steerTempUnavailable":  alert("Take Control", "Steer Temporarily Unavailable",           ET.WARNING,           "steerRequired", "chimeDouble",   .4, 2., 3.),
+    "brakeUnavailable":      alert("Take Control Immediately","Brake Error: Restart the Car", ET.IMMEDIATE_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
+    "gasUnavailable":        alert("Take Control Immediately","Gas Error: Restart the Car",   ET.IMMEDIATE_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
+    "wrongGear":             alert("Take Control Immediately","Gear not D",                   ET.SOFT_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
+    "reverseGear":           alert("Take Control Immediately","Car in Reverse",               ET.IMMEDIATE_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
+    "doorOpen":              alert("Take Control Immediately","Door Open",                    ET.SOFT_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
+    "seatbeltNotLatched":    alert("Take Control Immediately","Seatbelt Unlatched",           ET.SOFT_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
+    "espDisabled":           alert("Take Control Immediately","ESP Off",                      ET.SOFT_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
+    "cruiseDisabled":        alert("Take Control Immediately","Cruise Is Off",                ET.IMMEDIATE_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
+    "wrongCarMode":          alert("Comma Unavailable","Main Switch Off",                     ET.NO_ENTRY,     None, "chimeDouble", .4, 0., 3.),
+    "outOfSpace":            alert("Comma Unavailable","Out of Space",                        ET.NO_ENTRY,     None, "chimeDouble", .4, 0., 3.),
+    "dataNeeded":            alert("Comma Unavailable","Data needed for calibration. Upload drive, try again", ET.NO_ENTRY, None, "chimeDouble", .4, 0., 3.),
+    "ethicalDilemma":        alert("Take Control Immediately","Ethical Dilemma Detected",     ET.IMMEDIATE_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
+    "startup":               alert("Always Keep Hands on Wheel","Be Ready to Take Over Any Time", ET.NO_ENTRY, None, None, 0., 0., 15.),
  }
  def __init__(self):
    self.activealerts = []
@ -71,25 +76,27 @@ class AlertManager(object):
    return len(self.activealerts) > 0

  def alertShouldSoftDisable(self):
-    return len(self.activealerts) > 0 and self.activealerts[0].alert_type == ET.SOFT_DISABLE
+    return len(self.activealerts) > 0 and any(a.alert_type == ET.SOFT_DISABLE for a in self.activealerts)

  def alertShouldDisable(self):
-    return len(self.activealerts) > 0 and self.activealerts[0].alert_type >= ET.IMMEDIATE_DISABLE
+    return len(self.activealerts) > 0 and any(a.alert_type in [ET.IMMEDIATE_DISABLE, ET.USER_DISABLE] for a in self.activealerts)

-  def add(self, alert_type, enabled = True):
+  def add(self, alert_type, enabled=True, extra_text=''):
    alert_type = str(alert_type)
-    this_alert = self.alerts[alert_type]
-
-    # downgrade the alert if we aren't enabled
-    if not enabled and this_alert.alert_type > ET.NO_ENTRY:
+    this_alert = copy.copy(self.alerts[alert_type])
+    this_alert.alert_text_2 += extra_text
+    # downgrade the alert if we aren't enabled, except if it's FCW, which remains the same 
+    # TODO: remove this 'if' by adding more alerts
+    if not enabled and this_alert.alert_type in [ET.WARNING, ET.SOFT_DISABLE, ET.IMMEDIATE_DISABLE] \
+      and this_alert != self.alerts['fcw']:
      this_alert = alert("Comma Unavailable" if this_alert.alert_text_1 != "" else "", this_alert.alert_text_2, ET.NO_ENTRY, None, "chimeDouble", .4, 0., 3.)

    # ignore no entries if we are enabled 
-    if enabled and this_alert.alert_type < ET.WARNING:
+    if enabled and this_alert.alert_type in [ET.ENABLE, ET.NO_ENTRY]:
      return

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

 def calc_curvature(v_ego, angle_steers, CP, angle_offset=0):
  deg_to_rad = np.pi/180.
@ -8,15 +8,28 @@ def calc_curvature(v_ego, angle_steers, CP, angle_offset=0):
  curvature = angle_steers_rad/(CP.steerRatio * CP.wheelBase * (1. + CP.slipFactor * v_ego**2))
  return curvature

-def calc_d_lookahead(v_ego):
+_K_CURV_V = [1., 0.6]
+_K_CURV_BP = [0., 0.002]
+
+def calc_d_lookahead(v_ego, d_poly):
  #*** this function computes how far too look for lateral control
-  # howfar we look ahead is function of speed
+  # howfar we look ahead is function of speed and how much curvy is the path
  offset_lookahead = 1.
-  coeff_lookahead = 4.4
+  k_lookahead = 7.
+  # integrate abs value of second derivative of poly to get a measure of path curvature
+  pts_len = 50.  # m
+  if len(d_poly)>0:
+    pts = np.polyval([6*d_poly[0], 2*d_poly[1]], np.arange(0, pts_len))
+  else:
+    pts = 0.
+  curv = np.sum(np.abs(pts))/pts_len
+
+  k_curv = interp(curv, _K_CURV_BP, _K_CURV_V)
+
  # sqrt on speed is needed to keep, for a given curvature, the y_offset
  # proportional to speed. Indeed, y_offset is prop to d_lookahead^2
-  # 26m at 25m/s
-  d_lookahead = offset_lookahead + math.sqrt(max(v_ego, 0)) * coeff_lookahead
+  # 36m at 25m/s
+  d_lookahead = offset_lookahead + math.sqrt(max(v_ego, 0)) * k_lookahead * k_curv
  return d_lookahead

 def calc_lookahead_offset(v_ego, angle_steers, d_lookahead, CP, angle_offset):
@ -101,8 +114,8 @@ class LatControl(object):

    steer_max = 1.0

-    # how far we look ahead is function of speed
-    d_lookahead = calc_d_lookahead(v_ego)
+    # how far we look ahead is function of speed and desired path
+    d_lookahead = calc_d_lookahead(v_ego, d_poly)

    # calculate actual offset at the lookahead point
    self.y_actual, _ = calc_lookahead_offset(v_ego, angle_steers,
--- a/selfdrive/controls/lib/pathplanner.py
+++ b/selfdrive/controls/lib/pathplanner.py
@ -39,21 +39,35 @@ def calc_desired_path(l_poly, r_poly, p_poly, l_prob, r_prob, p_prob, speed):
  d_poly =  list((c_poly*c_prob + p_poly*p_prob*p_weight ) / (c_prob + p_prob*p_weight))
  return d_poly, c_poly, c_prob

-class PathPlanner(object):
+class OptPathPlanner(object):
  def __init__(self, model):
    self.model = model
    self.dead = True
    self.d_poly = [0., 0., 0., 0.]
    self.last_model = 0.
-    self.logMonoTime = 0
+    self._path_pinv = compute_path_pinv()
+
+  def update(self, cur_time, v_ego, md):
+    if md is not None:
+      # simple compute of the center of the lane
+      pts = [(x+y)/2 for x,y in zip(md.model.leftLane.points, md.model.rightLane.points)]
+      self.d_poly = model_polyfit(pts, self._path_pinv)
+
+      self.last_model = cur_time
+      self.dead = False
+    elif cur_time - self.last_model > 0.5:
+      self.dead = True
+
+class PathPlanner(object):
+  def __init__(self):
+    self.dead = True
+    self.d_poly = [0., 0., 0., 0.]
+    self.last_model = 0.
    self.lead_dist, self.lead_prob, self.lead_var = 0, 0, 1
    self._path_pinv = compute_path_pinv()

-  def update(self, cur_time, v_ego):
-    md = messaging.recv_sock(self.model)
-
+  def update(self, cur_time, v_ego, md):
    if md is not None:
-      self.logMonoTime = md.logMonoTime
      p_poly = model_polyfit(md.model.path.points, self._path_pinv)       # predicted path
      l_poly = model_polyfit(md.model.leftLane.points, self._path_pinv)   # left line
      r_poly = model_polyfit(md.model.rightLane.points, self._path_pinv)  # right line
--- a/selfdrive/controls/lib/planner.py
+++ b/selfdrive/controls/lib/planner.py
@ -0,0 +1,84 @@
+#!/usr/bin/env python
+import os
+import zmq
+import numpy as np
+import selfdrive.messaging as messaging
+
+from selfdrive.services import service_list
+from common.realtime import sec_since_boot
+from common.params import Params
+
+from selfdrive.swaglog import cloudlog
+from cereal import car
+
+from selfdrive.controls.lib.pathplanner import OptPathPlanner
+from selfdrive.controls.lib.pathplanner import PathPlanner
+from selfdrive.controls.lib.adaptivecruise import AdaptiveCruise
+from selfdrive.controls.lib.fcw import ForwardCollisionWarning
+
+_DT = 0.01   # 100Hz
+
+class Planner(object):
+  def __init__(self, CP):
+    context = zmq.Context()
+    self.CP = CP
+    self.live20 = messaging.sub_sock(context, service_list['live20'].port)
+    self.model = messaging.sub_sock(context, service_list['model'].port)
+  
+    self.plan = messaging.pub_sock(context, service_list['plan'].port)
+  
+    self.last_md_ts = 0
+    self.last_l20_ts = 0
+
+    if os.getenv("OPT") is not None:
+      self.PP = OptPathPlanner(self.model)
+    else:
+      self.PP = PathPlanner()
+    self.AC = AdaptiveCruise()
+    self.FCW = ForwardCollisionWarning(_DT)
+
+  # this runs whenever we get a packet that can change the plan
+  
+  def update(self, CS, LoC):
+    cur_time = sec_since_boot()
+
+    md = messaging.recv_sock(self.model)
+    if md is not None:
+      self.last_md_ts = md.logMonoTime
+    l20 = messaging.recv_sock(self.live20)
+    if l20 is not None:
+      self.last_l20_ts = l20.logMonoTime
+
+    self.PP.update(cur_time, CS.vEgo, md)
+
+    # LoC.v_pid -> CS.vEgo
+    # TODO: is this change okay?
+    self.AC.update(cur_time, CS.vEgo, CS.steeringAngle, LoC.v_pid, self.CP, l20)
+
+    # **** send the plan ****
+    plan_send = messaging.new_message()
+    plan_send.init('plan')
+
+    plan_send.plan.mdMonoTime = self.last_md_ts
+    plan_send.plan.l20MonoTime = self.last_l20_ts
+
+    # lateral plan
+    plan_send.plan.lateralValid = not self.PP.dead
+    if plan_send.plan.lateralValid:
+      plan_send.plan.dPoly = map(float, self.PP.d_poly)
+
+    # longitudal plan
+    plan_send.plan.longitudinalValid = not self.AC.dead
+    if plan_send.plan.longitudinalValid:
+      plan_send.plan.vTarget = float(self.AC.v_target_lead)
+      plan_send.plan.aTargetMin = float(self.AC.a_target[0])
+      plan_send.plan.aTargetMax = float(self.AC.a_target[1])
+      plan_send.plan.jerkFactor = float(self.AC.jerk_factor)
+      plan_send.plan.hasLead = self.AC.has_lead
+
+    # compute risk of collision events: fcw
+    self.FCW.process(CS, self.AC)
+    plan_send.plan.fcw = bool(self.FCW.active)
+
+    self.plan.send(plan_send.to_bytes())
+    return plan_send
--- a/selfdrive/controls/lib/radar_helpers.py
+++ b/selfdrive/controls/lib/radar_helpers.py
@ -205,7 +205,7 @@ class Cluster(object):
    lead.vLeadK = float(self.vLeadK)
    lead.aLeadK = float(self.aLeadK)
    lead.status = True
-    lead.fcw = False
+    lead.fcw = self.is_potential_fcw()

  def __str__(self):
    ret = "x: %7.2f  y: %7.2f  v: %7.2f  a: %7.2f" % (self.dRel, self.yRel, self.vRel, self.aRel)
@ -240,18 +240,18 @@ class Cluster(object):

    d_path = max(d_path + lat_corr, 0)

-    if d_path < 1.5 and not self.stationary and not self.oncoming:
-      return True
-    else:
-      return False
+    return d_path < 1.5 and not self.stationary and not self.oncoming

  def is_potential_lead2(self, lead_clusters):
    if len(lead_clusters) > 0:
      lead_cluster = lead_clusters[0]
-      # check if the new lead is too close and roughly at the same speed of the first lead: it might just be the second axle of the same vehicle
-      if (self.dRel - lead_cluster.dRel) < 8. and abs(self.vRel - lead_cluster.vRel) < 1.:
-        return False
-      else:
-        return True
+      # check if the new lead is too close and roughly at the same speed of the first lead:
+      # it might just be the second axle of the same vehicle
+      return (self.dRel - lead_cluster.dRel) > 8. or abs(self.vRel - lead_cluster.vRel) > 1.
    else:
      return False
+
+  def is_potential_fcw(self):
+    # is this cluster trustrable enough for triggering fcw?
+    # fcw can trigger only on clusters that have been fused vision model for at least 20 frames
+    return self.vision_cnt >= 20
--- a/selfdrive/controls/plannerd.py
+++ b/selfdrive/controls/plannerd.py
@ -1,81 +0,0 @@
-#!/usr/bin/env python
-import os
-import zmq
-import numpy as np
-import selfdrive.messaging as messaging
-
-from selfdrive.services import service_list
-from common.realtime import sec_since_boot, set_realtime_priority
-from common.params import Params
-
-from selfdrive.swaglog import cloudlog
-from cereal import car
-
-from selfdrive.controls.lib.pathplanner import PathPlanner
-from selfdrive.controls.lib.adaptivecruise import AdaptiveCruise
-
-def plannerd_thread(gctx):
-  context = zmq.Context()
-  poller = zmq.Poller()
-
-  carstate = messaging.sub_sock(context, service_list['carState'].port, poller)
-  live20 = messaging.sub_sock(context, service_list['live20'].port)
-  model = messaging.sub_sock(context, service_list['model'].port)
-
-  plan = messaging.pub_sock(context, service_list['plan'].port)
-
-  # wait for stats about the car to come in from controls
-  cloudlog.info("plannerd is waiting for CarParams")
-  CP = car.CarParams.from_bytes(Params().get("CarParams", block=True))
-  cloudlog.info("plannerd got CarParams")
-
-  CS = None
-  PP = PathPlanner(model)
-  AC = AdaptiveCruise(live20)
-
-  # start the loop
-  set_realtime_priority(2)
-
-  # this runs whenever we get a packet that can change the plan
-  while True:
-    polld = poller.poll(timeout=1000)
-    for sock, mode in polld:
-      if mode != zmq.POLLIN or sock != carstate:
-        continue
-
-      cur_time = sec_since_boot()
-      CS = messaging.recv_sock(carstate).carState
-
-      PP.update(cur_time, CS.vEgo)
-
-      # LoC.v_pid -> CS.vEgo
-      # TODO: is this change okay?
-      AC.update(cur_time, CS.vEgo, CS.steeringAngle, CS.vEgo, CP)
-
-      # **** send the plan ****
-      plan_send = messaging.new_message()
-      plan_send.init('plan')
-
-      # lateral plan
-      plan_send.plan.lateralValid = not PP.dead
-      if plan_send.plan.lateralValid:
-        plan_send.plan.dPoly = map(float, PP.d_poly)
-
-      # longitudal plan
-      plan_send.plan.longitudinalValid = not AC.dead
-      if plan_send.plan.longitudinalValid:
-        plan_send.plan.vTarget = float(AC.v_target_lead)
-        plan_send.plan.aTargetMin = float(AC.a_target[0])
-        plan_send.plan.aTargetMax = float(AC.a_target[1])
-        plan_send.plan.jerkFactor = float(AC.jerk_factor)
-        plan_send.plan.hasLead = AC.has_lead
-
-      plan.send(plan_send.to_bytes())
-
-
-def main(gctx=None):
-  plannerd_thread(gctx)
-
-if __name__ == "__main__":
-  main()
-
--- a/selfdrive/controls/radard.py
+++ b/selfdrive/controls/radard.py
@ -20,6 +20,7 @@ from common.params import Params
 from common.realtime import sec_since_boot, set_realtime_priority, Ratekeeper
 from common.kalman.ekf import EKF, SimpleSensor

+VISION_ONLY = False

 #vision point
 DIMSV = 2
@ -62,9 +63,12 @@ def radard_thread(gctx=None):
  model = messaging.sub_sock(context, service_list['model'].port)
  live100 = messaging.sub_sock(context, service_list['live100'].port)

-  PP = PathPlanner(model)
+  PP = PathPlanner()
  RI = RadarInterface()

+  last_md_ts = 0
+  last_l100_ts = 0
+
  # *** publish live20 and liveTracks
  live20 = messaging.pub_sock(context, service_list['live20'].port)
  liveTracks = messaging.pub_sock(context, service_list['liveTracks'].port)
@ -109,18 +113,24 @@ def radard_thread(gctx=None):
      v_ego_array = np.append(v_ego_array, [[v_ego], [float(rk.frame)/rate]], 1)
      v_ego_array = v_ego_array[:, 1:]

+      last_l100_ts = l100.logMonoTime
+
    if v_ego is None:
      continue

+    md = messaging.recv_sock(model)
+    if md is not None:
+      last_md_ts = md.logMonoTime
+
    # *** get path prediction from the model ***
-    PP.update(sec_since_boot(), v_ego)
+    PP.update(sec_since_boot(), v_ego, md)

    # run kalman filter only if prob is high enough
    if PP.lead_prob > 0.7:
      ekfv.update(speedSensorV.read(PP.lead_dist, covar=PP.lead_var))
      ekfv.predict(tsv)
      ar_pts[VISION_POINT] = (float(ekfv.state[XV]), np.polyval(PP.d_poly, float(ekfv.state[XV])),
-                              float(ekfv.state[SPEEDV]), np.nan, PP.logMonoTime, np.nan, sec_since_boot())
+                              float(ekfv.state[SPEEDV]), np.nan, last_md_ts, np.nan, sec_since_boot())
    else:
      ekfv.state[XV] = PP.lead_dist
      ekfv.covar = (np.diag([PP.lead_var, ekfv.var_init]))
@ -142,7 +152,9 @@ def radard_thread(gctx=None):
    # *** compute the tracks ***
    for ids in ar_pts:
      # ignore the vision point for now
-      if ids == VISION_POINT:
+      if ids == VISION_POINT and not VISION_ONLY:
+        continue
+      elif ids != VISION_POINT and VISION_ONLY:
        continue
      rpt = ar_pts[ids]

@ -212,8 +224,9 @@ def radard_thread(gctx=None):
    # *** publish live20 ***
    dat = messaging.new_message()
    dat.init('live20')
-    dat.live20.mdMonoTime = PP.logMonoTime
+    dat.live20.mdMonoTime = last_md_ts
    dat.live20.canMonoTimes = list(rr.canMonoTimes)
+    dat.live20.l100MonoTime = last_l100_ts
    if lead_len > 0:
      lead_clusters[0].toLive20(dat.live20.leadOne)
      if lead2_len > 0:
--- a/selfdrive/debug/test_carcontroller.py
+++ b/selfdrive/debug/test_carcontroller.py
@ -1,17 +1,17 @@
 #!/usr/bin/env python
-from evdev import InputDevice
-from select import select
 import time
 import numpy as np
 import zmq
+from evdev import InputDevice
+from select import select

 from cereal import car
+from common.realtime import Ratekeeper

 import selfdrive.messaging as messaging
 from selfdrive.services import service_list
-from common.realtime import Ratekeeper
+from selfdrive.car import get_car

-from common.fingerprints import fingerprint

 if __name__ == "__main__":
  # ***** connect to joystick *****
@ -27,10 +27,7 @@ if __name__ == "__main__":
  logcan = messaging.sub_sock(context, service_list['can'].port)
  sendcan = messaging.pub_sock(context, service_list['sendcan'].port)

-  CP = fingerprint(logcan)
-  exec('from selfdrive.car.'+CP.carName+'.interface import CarInterface')
-
-  CI = CarInterface(CP, logcan, sendcan)
+  CI, CP = get_car(logcan, sendcan)

  rk = Ratekeeper(100)

--- a/selfdrive/debug/test_carstate.py
+++ b/selfdrive/debug/test_carstate.py
@ -5,7 +5,7 @@ import zmq
 import selfdrive.messaging as messaging
 from selfdrive.services import service_list

-from common.fingerprints import fingerprint
+from selfdrive.car import get_car

 def bpressed(CS, btype):
  for b in CS.buttonEvents:
@ -17,10 +17,7 @@ def test_loop():
  context = zmq.Context()
  logcan = messaging.sub_sock(context, service_list['can'].port)

-  CP = fingerprint(logcan)
-  exec('from selfdrive.car.'+CP.carName+'.interface import CarInterface')
-
-  CI = CarInterface(CP, logcan, None)
+  CI, CP = get_car(logcan)

  state = 0

--- a/selfdrive/loggerd/loggerd
+++ b/selfdrive/loggerd/loggerd
--- a/selfdrive/loggerd/uploader.py
+++ b/selfdrive/loggerd/uploader.py
@ -117,7 +117,12 @@ class Uploader(object):
      if name in ["rlog", "rlog.bz2"]:
        return (key, fn, 0)

-    # then upload camera files no not on wifi
+    # then upload compressed camera file
+    for name, key, fn in self.gen_upload_files():
+      if name in ["fcamera.hevc"]:
+        return (key, fn, 1)
+
+    # then upload other files
    for name, key, fn in self.gen_upload_files():
      if not name.endswith('.lock') and not name.endswith(".tmp"):
        return (key, fn, 1)
--- a/selfdrive/manager.py
+++ b/selfdrive/manager.py
@ -42,7 +42,6 @@ BASEDIR = os.path.join(os.path.dirname(os.path.realpath(__file__)), "../")
 managed_processes = {
  "uploader": "selfdrive.loggerd.uploader",
  "controlsd": "selfdrive.controls.controlsd",
-  "plannerd": "selfdrive.controls.plannerd",
  "radard": "selfdrive.controls.radard",
  "loggerd": ("loggerd", ["./loggerd"]),
  "logmessaged": "selfdrive.logmessaged",
@ -66,7 +65,6 @@ interrupt_processes = []

 car_started_processes = [
  'controlsd',
-  'plannerd',
  'loggerd',
  'sensord',
  'radard',
@ -345,8 +343,8 @@ def get_installed_apks():
 # optional, build the c++ binaries and preimport the python for speed
 def manager_prepare():
  # update submodules
-  system("cd %s && git submodule init" % BASEDIR)
-  system("cd %s && git submodule update" % BASEDIR)
+  system("cd %s && git submodule init panda opendbc pyextra" % BASEDIR)
+  system("cd %s && git submodule update panda opendbc pyextra" % BASEDIR)

  # build cereal first
  subprocess.check_call(["make", "-j4"], cwd=os.path.join(BASEDIR, "cereal"))
@ -452,9 +450,20 @@ def main():
  if params.get("IsRearViewMirror") is None:
    params.put("IsRearViewMirror", "1")

-  manager_init()
-  manager_prepare()
-  
+  # put something on screen while we set things up
+  if os.getenv("PREPAREONLY") is not None:
+    spinner_proc = None
+  else:
+    spinner_proc = subprocess.Popen(["./spinner", "loading openpilot..."],
+      cwd=os.path.join(BASEDIR, "selfdrive", "ui", "spinner"),
+      close_fds=True)
+  try:
+    manager_init()
+    manager_prepare()
+  finally:
+    if spinner_proc:
+      spinner_proc.terminate()
+
  if os.getenv("PREPAREONLY") is not None:
    sys.exit(0)

--- a/selfdrive/radar/nidec/interface.py
+++ b/selfdrive/radar/nidec/interface.py
@ -1,14 +1,21 @@
 #!/usr/bin/env python
+import os
 import numpy as np
+
 from selfdrive.car.honda.can_parser import CANParser
+from selfdrive.can.parser import CANParser as CANParserC
+
 from selfdrive.boardd.boardd import can_capnp_to_can_list

 from cereal import car
+from common.realtime import sec_since_boot

 import zmq
 from selfdrive.services import service_list
 import selfdrive.messaging as messaging

+NEW_CAN = os.getenv("OLD_CAN") is None
+
 def _create_radard_can_parser():
  dbc_f = 'acura_ilx_2016_nidec.dbc'
  radar_messages = range(0x430, 0x43A) + range(0x440, 0x446)
@ -17,7 +24,10 @@ def _create_radard_can_parser():
                [255] * 16 + [1] * 16 + [0] * 16 + [0] * 16)
  checks = zip(radar_messages, [20]*16)

-  return CANParser(dbc_f, signals, checks)
+  if NEW_CAN:
+    return CANParserC(os.path.splitext(dbc_f)[0], signals, checks, 1)
+  else:
+    return CANParser(dbc_f, signals, checks)

 class RadarInterface(object):
  def __init__(self):
@ -33,19 +43,28 @@ class RadarInterface(object):

  def update(self):
    canMonoTimes = []
-    can_pub_radar = []

-    # TODO: can hang if no packets show up
-    while 1:
-      for a in messaging.drain_sock(self.logcan, wait_for_one=True):
-        canMonoTimes.append(a.logMonoTime)
-        can_pub_radar.extend(can_capnp_to_can_list(a.can, [1, 3]))
+    if NEW_CAN:
+      updated_messages = set()
+      while 1:
+        tm = int(sec_since_boot() * 1e9)
+        updated_messages.update(self.rcp.update(tm, True))
+        if 0x445 in updated_messages:
+          break
+    else:
+      can_pub_radar = []

-      # only run on the 0x445 packets, used for timing
-      if any(x[0] == 0x445 for x in can_pub_radar):
-        break
+      # TODO: can hang if no packets show up
+      while 1:
+        for a in messaging.drain_sock(self.logcan, wait_for_one=True):
+          canMonoTimes.append(a.logMonoTime)
+          can_pub_radar.extend(can_capnp_to_can_list(a.can, [1, 3]))

-    updated_messages = self.rcp.update_can(can_pub_radar)
+        # only run on the 0x445 packets, used for timing
+        if any(x[0] == 0x445 for x in can_pub_radar):
+          break
+
+      updated_messages = self.rcp.update_can(can_pub_radar)

    ret = car.RadarState.new_message()
    errors = []
@ -56,6 +75,7 @@ class RadarInterface(object):

    for ii in updated_messages:
      cpt = self.rcp.vl[ii]
+      #print cpt
      if cpt['LONG_DIST'] < 255:
        if ii not in self.pts or cpt['NEW_TRACK']:
          self.pts[ii] = car.RadarState.RadarPoint.new_message()
--- a/selfdrive/sensord/.gitignore
+++ b/selfdrive/sensord/.gitignore
@ -0,0 +1 @@
+sensord
--- a/selfdrive/sensord/sensord
+++ b/selfdrive/sensord/sensord
--- a/selfdrive/service_list.yaml
+++ b/selfdrive/service_list.yaml
@ -41,6 +41,8 @@ navUpdate: [8028, true]
 qcomGnss: [8029, true]
 lidarPts: [8030, true]
 procLog: [8031, true]
+gpsLocationExternal: [8032, true]
+ubloxGnss: [8033, true]
 testModel: [8040, false]

 # manager -- base process to manage starting and stopping of all others
--- a/selfdrive/test/plant/plant_ui.py
+++ b/selfdrive/test/plant/plant_ui.py
@ -1,122 +0,0 @@
-#!/usr/bin/env python
-import pygame
-from plant import Plant
-from selfdrive.config import CruiseButtons
-import numpy as np
-import selfdrive.messaging as messaging
-import math
-
-CAR_WIDTH = 2.0
-CAR_LENGTH = 4.5
-
-METER = 8
-
-def rot_center(image, angle):
-  """rotate an image while keeping its center and size"""
-  orig_rect = image.get_rect()
-  rot_image = pygame.transform.rotate(image, angle)
-  rot_rect = orig_rect.copy()
-  rot_rect.center = rot_image.get_rect().center
-  rot_image = rot_image.subsurface(rot_rect).copy()
-  return rot_image
-
-def car_w_color(c):
-  car = pygame.Surface((METER*CAR_LENGTH, METER*CAR_LENGTH))
-  car.set_alpha(0)
-  car.fill((10,10,10))
-  car.set_alpha(128)
-  pygame.draw.rect(car, c, (METER*1.25, 0, METER*CAR_WIDTH, METER*CAR_LENGTH), 1)
-  return car
-
-if __name__ == "__main__":
-  pygame.init()
-  display = pygame.display.set_mode((1000, 1000))
-  pygame.display.set_caption('Plant UI')
-
-  car = car_w_color((255,0,255))
-  leadcar = car_w_color((255,0,0))
-
-  carx, cary, heading = 10.0, 50.0, 0.0
-
-  plant = Plant(100, distance_lead = 40.0)
-
-  control_offset = 2.0
-  control_pts = zip(np.arange(0, 100.0, 10.0), [50.0 + control_offset]*10)
-
-  def pt_to_car(pt):
-    x,y = pt
-    x -= carx
-    y -= cary
-    rx = x * math.cos(-heading) + y * -math.sin(-heading)
-    ry = x * math.sin(-heading) + y * math.cos(-heading)
-    return rx, ry
-
-  def pt_from_car(pt):
-    x,y = pt
-    rx = x * math.cos(heading) + y * -math.sin(heading)
-    ry = x * math.sin(heading) + y * math.cos(heading)
-    rx += carx
-    ry += cary
-    return rx, ry
-
-  while 1:
-    if plant.rk.frame%100 >= 20 and plant.rk.frame%100 <= 25:
-      cruise_buttons = CruiseButtons.RES_ACCEL
-    else:
-      cruise_buttons = 0
-
-    md = messaging.new_message()
-    md.init('model')
-    md.model.frameId = 0
-    for x in [md.model.path, md.model.leftLane, md.model.rightLane]:
-      x.points = [0.0]*50
-      x.prob = 0.0
-      x.std = 1.0
-
-    car_pts = map(pt_to_car, control_pts)
-
-    print car_pts
-
-    car_poly = np.polyfit([x[0] for x in car_pts], [x[1] for x in car_pts], 3)
-    md.model.path.points = np.polyval(car_poly, np.arange(0, 50)).tolist()
-    md.model.path.prob = 1.0
-    Plant.model.send(md.to_bytes())
-
-    plant.step(cruise_buttons = cruise_buttons, v_lead = 2.0, publish_model = False)
-
-    display.fill((10,10,10))
-
-    carx += plant.speed * plant.ts * math.cos(heading)
-    cary += plant.speed * plant.ts * math.sin(heading)
-
-    # positive steering angle = steering right
-    print plant.angle_steer
-    heading += plant.angle_steer * plant.ts
-    print heading
-
-    # draw my car
-    display.blit(pygame.transform.rotate(car, 90-math.degrees(heading)), (carx*METER, cary*METER))
-
-    # draw control pts
-    for x,y in control_pts:
-      pygame.draw.circle(display, (255,255,0), (int(x * METER),int(y * METER)), 2)
-
-    # draw path 
-    path_pts = zip(np.arange(0, 50), md.model.path.points)
-    
-    for x,y in path_pts:
-      x,y = pt_from_car((x,y))
-      pygame.draw.circle(display, (0,255,0), (int(x * METER),int(y * METER)), 1)
-
-    """
-    # draw lead car
-    dl = (plant.distance_lead - plant.distance) + 4.5
-    lx = carx + dl * math.cos(heading)
-    ly = cary + dl * math.sin(heading)
-
-    display.blit(pygame.transform.rotate(leadcar, 90-math.degrees(heading)), (lx*METER, ly*METER))
-    """
-
-    pygame.display.flip()
-
-
--- a/selfdrive/test/plant/runtest.sh
+++ b/selfdrive/test/plant/runtest.sh
@ -1,12 +1,14 @@
 #!/bin/bash
+
+export OPTEST=1
+export OLD_CAN=1
+
 pushd ../../controls
 ./controlsd.py &
 pid1=$!
 ./radard.py &
 pid2=$!
-./plannerd.py &
-pid3=$!
-trap "trap - SIGTERM && kill $pid1 && kill $pid2 && kill $pid3" SIGINT SIGTERM EXIT
+trap "trap - SIGTERM && kill $pid1 && kill $pid2" SIGINT SIGTERM EXIT
 popd
 mkdir -p out
 MPLBACKEND=svg ./runtracks.py out
--- a/selfdrive/test/test_openpilot.py
+++ b/selfdrive/test/test_openpilot.py
@ -6,7 +6,6 @@ from selfdrive.manager import manager_init, manager_prepare
 from selfdrive.manager import start_managed_process, kill_managed_process, get_running
 from selfdrive.manager import manage_baseui
 from selfdrive.config import CruiseButtons
-from selfdrive.test.plant.plant import Plant
 from functools import wraps
 import time

@ -44,8 +43,10 @@ def with_processes(processes):
  return wrapper

@phone_only
-@with_processes(['controlsd', 'radard', 'plannerd'])
+@with_processes(['controlsd', 'radard'])
 def test_controls():
+  from selfdrive.test.plant.plant import Plant
+
  # start the fake car for 2 seconds
  plant = Plant(100)
  for i in range(200):
--- a/selfdrive/ui/.gitignore
+++ b/selfdrive/ui/.gitignore
@ -0,0 +1 @@
+ui
--- a/selfdrive/ui/spinner/spinner
+++ b/selfdrive/ui/spinner/spinner
--- a/selfdrive/ui/spinner/spinner.c
+++ b/selfdrive/ui/spinner/spinner.c
@ -0,0 +1,77 @@
+#include <stdio.h>
+#include <stdlib.h>
+#include <stdbool.h>
+#include <math.h>
+#include <unistd.h>
+#include <assert.h>
+
+#include <GLES3/gl3.h>
+#include <EGL/eglext.h>
+
+#include "nanovg.h"
+#define NANOVG_GLES3_IMPLEMENTATION
+#include "nanovg_gl.h"
+#include "nanovg_gl_utils.h"
+
+#include "common/framebuffer.h"
+
+
+int main(int argc, char** argv) {
+  int err;
+
+  const char* spintext = NULL;
+  if (argc >= 2) {
+    spintext = argv[1];
+  }
+
+  // spinner
+  int fb_w, fb_h;
+  EGLDisplay display;
+  EGLSurface surface;
+  FramebufferState *fb = framebuffer_init("spinner", 0x00001000, false,
+                     &display, &surface, &fb_w, &fb_h);
+  assert(fb);
+
+  NVGcontext *vg = nvgCreateGLES3(NVG_ANTIALIAS | NVG_STENCIL_STROKES);
+  assert(vg);
+
+  int font = nvgCreateFont(vg, "Bold", "../../assets/courbd.ttf");
+  assert(font >= 0);
+
+  for (int cnt = 0; ; cnt++) {
+    glClearColor(0.1, 0.1, 0.1, 1.0);
+    glClear(GL_STENCIL_BUFFER_BIT | GL_COLOR_BUFFER_BIT);
+
+    glEnable(GL_BLEND);
+    glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
+    nvgBeginFrame(vg, fb_w, fb_h, 1.0f);
+
+
+    for (int k=0; k<3; k++) {
+      float ang = (2*M_PI * (float)cnt / 120.0) + (k / 3.0) * 2*M_PI;
+
+      nvgBeginPath(vg);
+        nvgStrokeColor(vg, nvgRGBA(255, 255, 255, 255));
+        nvgStrokeWidth(vg, 5);
+        
+        nvgMoveTo(vg, fb_w/2 + 50 * cosf(ang), fb_h/2 + 50 * sinf(ang));
+        nvgLineTo(vg, fb_w/2 + 15 * cosf(ang), fb_h/2 + 15 * sinf(ang));
+        nvgMoveTo(vg, fb_w/2 - 15 * cosf(ang), fb_h/2 - 15 * sinf(ang));
+        nvgLineTo(vg, fb_w/2 - 50 * cosf(ang), fb_h/2 - 50 * sinf(ang));
+      nvgStroke(vg);
+    }
+
+    if (spintext) {
+      nvgTextAlign(vg, NVG_ALIGN_CENTER | NVG_ALIGN_TOP);
+      nvgFontSize(vg, 96.0f);
+      nvgText(vg, fb_w / 2, fb_h*2/3, spintext, NULL);      
+    }
+
+    nvgEndFrame(vg);
+
+    eglSwapBuffers(display, surface);
+    assert(glGetError() == GL_NO_ERROR);
+  }
+
+  return 0;
+}
--- a/selfdrive/ui/ui.c
+++ b/selfdrive/ui/ui.c
@ -61,6 +61,7 @@ typedef struct UIScene {
  uint8_t *bgr_front_ptr;
  int front_box_x, front_box_y, front_box_width, front_box_height;

+  uint64_t alert_ts;
  char alert_text1[1024];
  char alert_text2[1024];

@ -802,6 +803,11 @@ static void ui_draw_vision(UIState *s) {
 static void ui_draw_alerts(UIState *s) {
  const UIScene *scene = &s->scene;

+  // dont draw alerts that are outdated by > 20 secs
+  if ((nanos_since_boot() - scene->alert_ts) >= 20000000000ULL) {
+    return;
+  }
+
  glEnable(GL_BLEND);
  glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);

@ -1059,6 +1065,9 @@ static void ui_update(UIState *s) {
          s->scene.alert_text2[0] = '\0';
        }
        s->scene.awareness_status = datad.awarenessStatus;
+
+        s->scene.alert_ts = eventd.logMonoTime;
+
      } else if (eventd.which == cereal_Event_live20) {
        struct cereal_Live20Data datad;
        cereal_read_Live20Data(&datad, eventd.live20);
@ -1220,6 +1229,8 @@ int main() {
    usleep(30000);
  }

+  set_awake(s, true);
+
  err = pthread_join(connect_thread_handle, NULL);
  assert(err == 0);

--- a/selfdrive/visiond/visiond
+++ b/selfdrive/visiond/visiond
				`@ -0,0 +1 @@`
				`Subproject commit e0738376db27d208603d7e63dd465e003ca06325`