openpilot v0.3.4 release

old-commit-hash: 6f46f988d9

.gitignore

@@ -1,6 +1,7 @@
 .DS_Store
 .tags
 .ipynb_checkpoints
+.idea
 model2.png
 
 *.d

.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

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 && \
+RUN pip install numpy==1.11.2 scipy==0.18.1 matplotlib
-  /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
 
 COPY requirements_openpilot.txt /tmp/
 RUN pip install -r /tmp/requirements_openpilot.txt

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.**

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

apk/com.baseui.apk

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:027af259a0b06dc7a719241a74ed9d2d63c13beb49905db1008a908afdbdb1a3
+oid sha256:31f6d7340f0beed300d7ba8789dcfed8d8fd70aeb815bd430f031587c026b9b2
-size 8993895
+size 9479160

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"))
+capnp.remove_import_hook()
-car = capnp.load(os.path.join(CEREAL_PATH, "car.capnp"))
+
+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"))

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 \
+GENS := gen/cpp/car.capnp.c++ gen/cpp/log.capnp.c++
-	      gen/cpp/car.capnp.c++ gen/cpp/log.capnp.c++
+
 
-# Dont build java on the phone...
 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...
 ifeq ($(UNAME_M),x86_64)
 	GENS += gen/java/Car.java gen/java/Log.java
 endif
 
+endif
+
 .PHONY: all
 all: $(GENS)
 

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;
     # ...
   }

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;
   }
 }

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

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
+  if os.getenv("SIMULATOR") is not None or logcan is None:
-    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]
-
+  return (candidate_cars[0], finger)
-  # 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

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

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

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()

opendbc

@@ -1 +1 @@
-Subproject commit d584cdd46ca398aace9f1a7ee6c967a8c7602e8e
+Subproject commit b77861eb00d45e25af501f78bbed155d2df06159

pyextra

@@ -0,0 +1 @@
+Subproject commit e0738376db27d208603d7e63dd465e003ca06325

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

selfdrive/boardd/.gitignore

@@ -0,0 +1 @@
+boardd

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);
 }
-

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()
-

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

selfdrive/can/dbc_out/.gitignore

@@ -0,0 +1,2 @@
+*.cc
+

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}})

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

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)

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

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)

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

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)
 

selfdrive/car/honda/carstate.py

@@ -1,15 +1,22 @@
+import os
+import time
+
 import common.numpy_fast as np
+from common.realtime import sec_since_boot
 
 import selfdrive.messaging as messaging
-from common.realtime import sec_since_boot
 
 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

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 .carstate import CarState
-from selfdrive.car.honda.carcontroller import CarController, AH
+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):
+    if NEW_CAN:
-      canMonoTimes.append(a.logMonoTime)
+      self.CS.update(can_pub_main)
-      can_pub_main.extend(can_capnp_to_can_list(a.can, [0,2]))
+    else:
-      if self.CS.accord:
+      for a in messaging.drain_sock(self.logcan):
-        self.accord_msg.extend(can_capnp_to_can_list(a.can, [9]))
+        canMonoTimes.append(a.logMonoTime)
-        self.accord_msg = self.accord_msg[-1:]
+        can_pub_main.extend(can_capnp_to_can_list(a.can, [0,0x80]))
-    self.CS.update(can_pub_main)
+        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:

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.
+  // fsync to force persist the changes.
-  result = rename(tmp_path, path);
+  result = fsync(tmp_fd);
   if (result < 0) {
     goto cleanup;
   }
 
-  // fsync to force persist the changes.
+  // Move temp into place.
-  result = fsync(tmp_fd);
+  result = rename(tmp_path, path);
+
 cleanup:
   // Release lock.
   if (lock_fd >= 0) {

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);

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;
   }

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);
 
 

selfdrive/common/utilpp.h

@@ -5,6 +5,7 @@
 #include <unistd.h>
 
 #include <string>
+#include <memory>
 #include <sstream>
 #include <fstream>
 

selfdrive/common/version.h

@@ -1 +1 @@
-#define OPENPILOT_VERSION "0.3.3"
+#define OPENPILOT_VERSION "0.3.4"

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
+AWARENESS_TIME = 360.      # 6 minutes limit without user touching steering wheels
-  # *** log ***
+AWARENESS_PRE_TIME = 20.   # a first alert is issued 20s before start decelerating the car
-  context = zmq.Context()
+AWARENESS_DECEL = -0.2     # car smoothly decel at .2m/s^2 when user is distracted
-  live100 = messaging.pub_sock(context, service_list['live100'].port)
+
-  carstate = messaging.pub_sock(context, service_list['carState'].port)
+# class Cal
-  carcontrol = messaging.pub_sock(context, service_list['carControl'].port)
+class Calibration:
-
+  UNCALIBRATED = 0
-  thermal = messaging.sub_sock(context, service_list['thermal'].port)
+  CALIBRATED = 1
-  health = messaging.sub_sock(context, service_list['health'].port)
+  INVALID = 2
-  plan_sock = messaging.sub_sock(context, service_list['plan'].port)
+
-
+# to be used
-  logcan = messaging.sub_sock(context, service_list['can'].port)
+class State():
-
+  DISABLED = 0
-  # connects to can
+  ENABLED = 1
-  CP = fingerprint(logcan)
+  SOFT_DISABLE = 2
-
+
-  # import the car from the fingerprint
+class Controls(object):
-  cloudlog.info("controlsd is importing %s", CP.carName)
+  def __init__(self, gctx, rate=100):
-  exec('from selfdrive.car.'+CP.carName+'.interface import CarInterface')
+    self.rate = rate
-
+
-  sendcan = messaging.pub_sock(context, service_list['sendcan'].port)
+    # *** log ***
-  CI = CarInterface(CP, logcan, sendcan)
+    context = zmq.Context()
-
+
-  # write CarParams
+    # pub
-  params = Params()
+    self.live100 = messaging.pub_sock(context, service_list['live100'].port)
-  params.put("CarParams", CP.to_bytes())
+    self.carstate = messaging.pub_sock(context, service_list['carState'].port)
-
+    self.carcontrol = messaging.pub_sock(context, service_list['carControl'].port)
-  AM = AlertManager()
+    sendcan = messaging.pub_sock(context, service_list['sendcan'].port)
-
+ 
-  LoC = LongControl()
+    # sub
-  LaC = LatControl()
+    self.thermal = messaging.sub_sock(context, service_list['thermal'].port)
-
+    self.health = messaging.sub_sock(context, service_list['health'].port)
-  # fake plan
+    logcan = messaging.sub_sock(context, service_list['can'].port)
-  plan = log.Plan.new_message()
+    self.cal = messaging.sub_sock(context, service_list['liveCalibration'].port)
-  plan.lateralValid = False
+    
-  plan.longitudinalValid = False
+    self.CC = car.CarControl.new_message()
-  last_plan_time = 0
+    self.CI, self.CP = get_car(logcan, sendcan)
-
+    self.PL = Planner(self.CP)
-  # controls enabled state
+    self.AM = AlertManager()
-  enabled = False
+    self.LoC = LongControl()
-  last_enable_request = 0
+    self.LaC = LatControl()
-
+  
-  # learned angle offset
+    # write CarParams
-  angle_offset = 0
+    params = Params()
-
+    params.put("CarParams", self.CP.to_bytes())
-  # rear view camera state
+  
-  rear_view_toggle = False
+    # fake plan
-  rear_view_allowed = bool(params.get("IsRearViewMirror"))
+    self.plan_ts = 0
-
+    self.plan = log.Plan.new_message()
-  v_cruise_kph = 255
+    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()
+
+    self.prof.checkpoint("CarInterface")
 
-  # 0.0 - 1.0
+    # *** thermal checking logic ***
-  awareness_status = 0.0
+    # thermal data, checked every second
-
+    td = messaging.recv_sock(self.thermal)
-  soft_disable_timer = None
+    if td is not None:
-
+      # Check temperature.
-  # Is cpu temp too high to enable?
+      self.overtemp = any(
-  overtemp = False
+          t > 950
-  free_space = 1.0
+          for t in (td.thermal.cpu0, td.thermal.cpu1, td.thermal.cpu2,
-
+                    td.thermal.cpu3, td.thermal.mem, td.thermal.gpu))
-  # start the loop
+      # under 15% of space free
-  set_realtime_priority(2)
+      self.free_space = td.thermal.freeSpace
-
-  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
+    # read calibration status
-    cs_send = messaging.new_message()
+    cal = messaging.recv_sock(self.cal)
-    cs_send.init('carState')
+    if cal is not None:
-    cs_send.carState = CS    # copy?
+      self.cal_status = cal.liveCalibration.calStatus
-    carstate.send(cs_send.to_bytes())
+      self.cal_perc = cal.liveCalibration.calPerc
+    
 
-    prof.checkpoint("CarState")
+  def state_transition(self):
+    pass # for now
 
+  def state_control(self):
+    
     # 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:
+    if self.CS.steeringPressed or not self.enabled:
-      awareness_status = 1.0
+      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 CS.buttonEvents:
+    for b in self.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:
+        if b.pressed and self.rear_view_allowed:
-          rear_view_toggle = True
+          self.rear_view_toggle = True
         else:
-          rear_view_toggle = False
+          self.rear_view_toggle = False
 
       if b.type == "altButton1" and b.pressed:
-        rear_view_toggle = not rear_view_toggle
+        self.rear_view_toggle = not self.rear_view_toggle
 
-      if not CP.enableCruise and enabled and not b.pressed:
+      if not self.CP.enableCruise and self.enabled and not b.pressed:
         if b.type == "accelCruise":
-          v_cruise_kph = v_cruise_kph - (v_cruise_kph % V_CRUISE_DELTA) + V_CRUISE_DELTA
+          self.v_cruise_kph -= (self.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
+          self.v_cruise_kph -= (self.v_cruise_kph % V_CRUISE_DELTA) + V_CRUISE_DELTA
-        v_cruise_kph = clip(v_cruise_kph, V_CRUISE_MIN, V_CRUISE_MAX)
+        self.v_cruise_kph = clip(self.v_cruise_kph, V_CRUISE_MIN, V_CRUISE_MAX)
 
-      if not enabled and b.type in ["accelCruise", "decelCruise"] and not b.pressed:
+      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:
-        AM.add("disable", enabled)
+        self.AM.add("disable", self.enabled)
-
-    prof.checkpoint("Buttons")
 
+    self.prof.checkpoint("Buttons")
+    
     # *** health checking logic ***
-    hh = messaging.recv_sock(health)
+    hh = messaging.recv_sock(self.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:
+      # TODO: this should be in state transition with a function follower logic
-        AM.add("controlsMismatch", enabled)
+      if not hh.health.controlsAllowed and self.enabled:
-
+        self.AM.add("controlsMismatch", self.enabled)
-    # *** thermal checking logic ***
-
-    # thermal data, checked every second
-    td = messaging.recv_sock(thermal)
-    if td is not None:
-      # Check temperature.
-      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
-
-    prof.checkpoint("Health")
 
     # disable if the pedals are pressed while engaged, this is a user disable
-    if enabled:
+    if self.enabled:
-      if CS.gasPressed or CS.brakePressed:
+      if self.CS.gasPressed or self.CS.brakePressed or not self.CS.cruiseState.mainOn:
-        AM.add("disable", enabled)
+        self.AM.add("disable", self.enabled)
 
-      # how did we get into this state?
+      # it can happen that car cruise disables while comma system is enabled: need to
-      if CP.enableCruise and not CS.cruiseState.enabled:
+      # keep braking if needed or if the speed is very low
-        AM.add("cruiseDisabled", enabled)
+      # 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:
+      if self.CS.gasPressed or self.CS.brakePressed:
-        AM.add("pedalPressed", enabled)
+        self.AM.add("pedalPressed", self.enabled)
         enable_request = False
       else:
-        print "enabled pressed at", cur_time
+        print "enabled pressed at", self.cur_time
-        last_enable_request = cur_time
+        self.last_enable_request = self.cur_time
 
       # don't engage with less than 15% free
-      if free_space < 0.15:
+      if self.free_space < 0.15:
-        AM.add("outOfSpace", enabled)
+        self.AM.add("outOfSpace", self.enabled)
         enable_request = False
 
-    if CP.enableCruise:
+    if self.CP.enableCruise:
-      enable_condition = ((cur_time - last_enable_request) < 0.2) and CS.cruiseState.enabled
+      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:
+    if self.CP.enableCruise and self.CS.cruiseState.enabled:
-      v_cruise_kph = CS.cruiseState.speed * CV.MS_TO_KPH
+      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)
+    plan_packet = self.PL.update(self.CS, self.LoC)
-    if new_plan is not None:
+    self.plan = plan_packet.plan
-      plan = new_plan.plan
+    self.plan_ts = plan_packet.logMonoTime
-      plan = plan.as_builder()  # plan can change in controls
+
-      last_plan_time = cur_time
+    # if user is not responsive to awareness alerts, then start a smooth deceleration
-
+    if self.awareness_status < -0.:
-    # check plan for timeout
+      self.plan.aTargetMax = min(self.plan.aTargetMax, AWARENESS_DECEL)
-    if cur_time - last_plan_time > 0.5:
+      self.plan.aTargetMin = min(self.plan.aTargetMin, self.plan.aTargetMax)
-      plan.lateralValid = False
+
-      plan.longitudinalValid = False
+    if enable_request or enable_condition or self.enabled:
-
-    # 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:
       # add all alerts from car
-      for alert in CS.errors:
+      for alert in self.CS.errors:
-        AM.add(alert, enabled)
+        self.AM.add(alert, self.enabled)
 
-      if not plan.longitudinalValid:
+      if not self.plan.longitudinalValid:
-        AM.add("radarCommIssue", enabled)
+        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:
+      if self.overtemp:
-        AM.add("overheat", enabled)
+        self.AM.add("overheat", self.enabled)
-
+
-    # *** angle offset learning ***
+    
-    if rk.frame % 5 == 2 and plan.lateralValid:
+    # *** angle offset learning *** 
-      # *** run this at 20hz again ***
+    if self.rk.frame % 5 == 2 and self.plan.lateralValid: 
-      angle_offset = learn_angle_offset(enabled, CS.vEgo, angle_offset, plan.dPoly, LaC.y_des, CS.steeringPressed)
+      # *** run this at 20hz again *** 
-
+      self.angle_offset = learn_angle_offset(self.enabled, self.CS.vEgo, self.angle_offset, 
-    # *** gas/brake PID loop ***
+                                             self.plan.dPoly, self.LaC.y_des, self.CS.steeringPressed) 
-    final_gas, final_brake = LoC.update(enabled, CS.vEgo, v_cruise_kph,
+
-                                        plan.vTarget,
+    # *** gas/brake PID loop *** 
-                                        [plan.aTargetMin, plan.aTargetMax],
+    final_gas, final_brake = self.LoC.update(self.enabled, self.CS.vEgo, self.v_cruise_kph, 
-                                        plan.jerkFactor, CP)
+                                        self.plan.vTarget, 
+                                        [self.plan.aTargetMin, self.plan.aTargetMax], 
+                                        self.plan.jerkFactor, self.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)
 
-    # *** steering PID loop ***
-    final_steer, sat_flag = LaC.update(enabled, CS.vEgo, CS.steeringAngle, CS.steeringPressed, plan.dPoly, angle_offset, CP)
-
-    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 self.enabled and self.AM.alertShouldSoftDisable():
-      if soft_disable_timer is None:
+      if self.soft_disable_timer is None:
-        soft_disable_timer = 3 * rate
+        self.soft_disable_timer = 3 * self.rate
-      elif soft_disable_timer == 0:
+      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)
+    self.CC.cruiseControl.override = True
-    CC.brake = float(final_brake)
+    # always cancel if we have an interceptor
-    CC.steeringTorque = float(final_steer)
+    self.CC.cruiseControl.cancel = bool(not self.CP.enableCruise or 
-
+                                        (not self.enabled and self.CS.cruiseState.enabled))
-    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
 
     # 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
+    self.CC.hudControl.setSpeed = float(self.v_cruise_kph * CV.KPH_TO_MS)
-    CC.hudControl.audibleAlert = audible_alert
+    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):
+    if not self.CI.apply(self.CC):
-      AM.add("controlsFailed", enabled)
+      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?
+    cc_send.carControl = copy(self.CC)
-    carcontrol.send(cc_send.to_bytes())
+    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.rearViewCam = ('reverseGear' in self.CS.errors and self.rear_view_allowed) or self.rear_view_toggle
-    dat.live100.alertText1 = alert_text_1
+    dat.live100.alertText1 = self.alert_text_1
-    dat.live100.alertText2 = alert_text_2
+    dat.live100.alertText2 = self.alert_text_2
-    dat.live100.awarenessStatus = max(awareness_status, 0.0) if enabled else 0.0
+    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.vEgo = self.CS.vEgo
-    dat.live100.angleSteers = CS.steeringAngle
+    dat.live100.angleSteers = self.CS.steeringAngle
-    dat.live100.steerOverride = CS.steeringPressed
+    dat.live100.steerOverride = self.CS.steeringPressed
 
     # longitudinal control state
-    dat.live100.vPid = float(LoC.v_pid)
+    dat.live100.vPid = float(self.LoC.v_pid)
-    dat.live100.vCruise = float(v_cruise_kph)
+    dat.live100.vCruise = float(self.v_cruise_kph)
-    dat.live100.upAccelCmd = float(LoC.Up_accel_cmd)
+    dat.live100.upAccelCmd = float(self.LoC.Up_accel_cmd)
-    dat.live100.uiAccelCmd = float(LoC.Ui_accel_cmd)
+    dat.live100.uiAccelCmd = float(self.LoC.Ui_accel_cmd)
 
     # lateral control state
-    dat.live100.yActual = float(LaC.y_actual)
+    dat.live100.yActual = float(self.LaC.y_actual)
-    dat.live100.yDes = float(LaC.y_des)
+    dat.live100.yDes = float(self.LaC.y_des)
-    dat.live100.upSteer = float(LaC.Up_steer)
+    dat.live100.upSteer = float(self.LaC.Up_steer)
-    dat.live100.uiSteer = float(LaC.Ui_steer)
+    dat.live100.uiSteer = float(self.LaC.Ui_steer)
 
     # processed radar state, should add a_pcm?
-    dat.live100.vTargetLead = float(plan.vTarget)
+    dat.live100.vTargetLead = float(self.plan.vTarget)
-    dat.live100.aTargetMin = float(plan.aTargetMin)
+    dat.live100.aTargetMin = float(self.plan.aTargetMin)
-    dat.live100.aTargetMax = float(plan.aTargetMax)
+    dat.live100.aTargetMax = float(self.plan.aTargetMax)
-    dat.live100.jerkFactor = float(plan.jerkFactor)
+    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():
+    if self.rk.keep_time():
-      prof.display()
+      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()
+ 

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):
+  def __init__(self):
-    self.live20 = live20
     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):
+  def update(self, cur_time, v_ego, angle_steers, v_pid, CP, l20):
-    l20 = messaging.recv_sock(self.live20)
     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

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
+  USER_DISABLE = 3
-  IMMEDIATE_DISABLE = 4
+  SOFT_DISABLE = 4
-  USER_DISABLE = 5
+  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.),
+    "enable":                alert("", "",                                                    ET.ENABLE,       None,               "beepSingle", .2, 0., 0.),
-    "disable":            alert("", "",                                            ET.USER_DISABLE, 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.),
+    "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.),
+    "preDriverDistracted":   alert("Take Control ",            "User Distracted",             ET.WARNING,      "steerRequired",    "chimeDouble", .4, 2., 3.),
-    "steerSaturated":     alert("Take Control", "Steering Limit Reached",         ET.WARNING,      "steerRequired", "chimeSingle", 1., 2., 3.),
+    "driverDistracted":      alert("Take Control to Regain Speed", "User Distracted",         ET.WARNING,      "steerRequired", "chimeRepeated", .5, .5, .5),
-    "overheat":           alert("Take Control Immediately", "System Overheated",   ET.SOFT_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
+    "steerSaturated":        alert("Take Control",             "Turn Exceeds Limit",          ET.WARNING,      "steerRequired", "chimeSingle", 1., 2., 3.),
-    "controlsMismatch":   alert("Take Control Immediately", "Controls Mismatch",   ET.IMMEDIATE_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
+    "overheat":              alert("Take Control Immediately", "System Overheated",           ET.SOFT_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
-    "radarCommIssue":     alert("Take Control Immediately", "Radar Comm Issue",    ET.IMMEDIATE_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
+    "controlsMismatch":      alert("Take Control Immediately", "Controls Mismatch",           ET.IMMEDIATE_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
-    "modelCommIssue":     alert("Take Control Immediately", "Model Comm Issue",    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.),
-    "controlsFailed":     alert("Take Control Immediately", "Controls Failed",     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.),
+    "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",          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.),
-    "steerTemporarilyUnavailable": alert("Take Control", "Steer Temporarily Unavailable",  ET.WARNING,   "steerRequired", "chimeRepeated", 1., 2., 3.),
+    "steerTempUnavailable":  alert("Take Control", "Steer Temporarily Unavailable",           ET.WARNING,           "steerRequired", "chimeDouble",   .4, 2., 3.),
-    "brakeUnavailable":   alert("Take Control Immediately","Brake Error",          ET.IMMEDIATE_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 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",            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.),
+    "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.),
+    "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.),
+    "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.),
+    "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.),
+    "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.),
+    "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.),
+    "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.),
+    "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.),
+    "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.),
+    "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.),
+    "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]
+    this_alert = copy.copy(self.alerts[alert_type])
-
+    this_alert.alert_text_2 += extra_text
-    # downgrade the alert if we aren't enabled
+    # downgrade the alert if we aren't enabled, except if it's FCW, which remains the same 
-    if not enabled and this_alert.alert_type > ET.NO_ENTRY:
+    # 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 new alert is higher priority, log it
-    if self.current_alert is None or self.current_alert.alert_text_2 != this_alert.alert_text_2:
+    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"

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

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
+  # 36m at 25m/s
-  d_lookahead = offset_lookahead + math.sqrt(max(v_ego, 0)) * coeff_lookahead
+  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
+    # how far we look ahead is function of speed and desired path
-    d_lookahead = calc_d_lookahead(v_ego)
+    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,

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.lead_dist, self.lead_prob, self.lead_var = 0, 0, 1
     self._path_pinv = compute_path_pinv()
 
-  def update(self, cur_time, v_ego):
+  def update(self, cur_time, v_ego, md):
-    md = messaging.recv_sock(self.model)
+    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):
     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

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

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 d_path < 1.5 and not self.stationary and not self.oncoming
-      return True
-    else:
-      return False
 
   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
+      # check if the new lead is too close and roughly at the same speed of the first lead:
-      if (self.dRel - lead_cluster.dRel) < 8. and abs(self.vRel - lead_cluster.vRel) < 1.:
+      # it might just be the second axle of the same vehicle
-        return False
+      return (self.dRel - lead_cluster.dRel) > 8. or abs(self.vRel - lead_cluster.vRel) > 1.
-      else:
-        return True
     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

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()
-

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:

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)
+  CI, CP = get_car(logcan, sendcan)
-  exec('from selfdrive.car.'+CP.carName+'.interface import CarInterface')
-
-  CI = CarInterface(CP, logcan, sendcan)
 
   rk = Ratekeeper(100)
 

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)
+  CI, CP = get_car(logcan)
-  exec('from selfdrive.car.'+CP.carName+'.interface import CarInterface')
-
-  CI = CarInterface(CP, logcan, None)
 
   state = 0
 

selfdrive/loggerd/loggerd

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:11408b2637323c15dfccdb0b6059cb45f9073ac700ab1efe233f43ccb61184a7
+oid sha256:706438daafdc304a4e31539dd6bd01f2ea6b349990756ff80d79728ae5a849a6
-size 1357296
+size 1365184

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)

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 init panda opendbc pyextra" % BASEDIR)
-  system("cd %s && git submodule update" % 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()
+  # put something on screen while we set things up
-  manager_prepare()
+  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)
 

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
+    if NEW_CAN:
-    while 1:
+      updated_messages = set()
-      for a in messaging.drain_sock(self.logcan, wait_for_one=True):
+      while 1:
-        canMonoTimes.append(a.logMonoTime)
+        tm = int(sec_since_boot() * 1e9)
-        can_pub_radar.extend(can_capnp_to_can_list(a.can, [1, 3]))
+        updated_messages.update(self.rcp.update(tm, True))
+        if 0x445 in updated_messages:
+          break
+    else:
+      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]))
 
-      # only run on the 0x445 packets, used for timing
+        # only run on the 0x445 packets, used for timing
-      if any(x[0] == 0x445 for x in can_pub_radar):
+        if any(x[0] == 0x445 for x in can_pub_radar):
-        break
+          break
 
-    updated_messages = self.rcp.update_can(can_pub_radar)
+      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()

selfdrive/sensord/.gitignore

@@ -0,0 +1 @@
+sensord

selfdrive/sensord/sensord

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:925d26ccc75ec4d7051b68fe98a929536c5224693f4e375fe20fb604299bc079
+oid sha256:58e476760c0f87f13b68b75d64860fb5c57b5509b4174d31f466074e0423e7eb
 size 918928

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

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()
-
-

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 &
+trap "trap - SIGTERM && kill $pid1 && kill $pid2" SIGINT SIGTERM EXIT
-pid3=$!
-trap "trap - SIGTERM && kill $pid1 && kill $pid2 && kill $pid3" SIGINT SIGTERM EXIT
 popd
 mkdir -p out
 MPLBACKEND=svg ./runtracks.py out

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):

selfdrive/ui/.gitignore

@@ -0,0 +1 @@
+ui

selfdrive/ui/spinner/spinner

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

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;
+}

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);
 

selfdrive/visiond/visiond

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:d4b8e99120b0e6f61b56e514e8b607ef797a6046c1cc2fffafe30c9231a65671
+oid sha256:61b61563109853f59a5764cc520fef8d9f0d8d2a845ebbcf9381b6a64761f655
-size 16370760
+size 16399784