openpilot release

selfdrive/boardd/Makefile

@@ -0,0 +1,73 @@
+CC = clang
+CXX = clang++
+
+ARCH := $(shell uname -m)
+
+PHONELIBS = ../../phonelibs
+
+WARN_FLAGS = -Werror=implicit-function-declaration \
+             -Werror=incompatible-pointer-types \
+             -Werror=int-conversion \
+             -Werror=return-type \
+             -Werror=format-extra-args
+
+CFLAGS = -std=gnu11 -g -fPIC -O2 $(WARN_FLAGS)
+CXXFLAGS = -std=c++11 -g -fPIC -O2 $(WARN_FLAGS)
+
+ZMQ_FLAGS = -I$(PHONELIBS)/zmq/aarch64/include
+ZMQ_LIBS = -L$(PHONELIBS)/zmq/aarch64/lib \
+           -l:libczmq.a -l:libzmq.a \
+           -lgnustl_shared
+
+CEREAL_FLAGS = -I$(PHONELIBS)/capnp-cpp/include
+CEREAL_LIBS = -L$(PHONELIBS)/capnp-cpp/aarch64/lib/ \
+              -l:libcapnp.a -l:libkj.a
+CEREAL_OBJS = ../../cereal/gen/c/log.capnp.o
+
+EXTRA_LIBS = -lusb
+
+ifeq ($(ARCH),x86_64)
+ZMQ_LIBS = -L$(HOME)/drive/external/zmq/lib/ \
+           -l:libczmq.a -l:libzmq.a
+CEREAL_LIBS = -L$(HOME)/drive/external/capnp/lib/ \
+              -l:libcapnp.a -l:libkj.a
+EXTRA_LIBS = -lusb-1.0 -lpthread
+endif
+
+
+OBJS = boardd.o \
+       log.capnp.o
+
+DEPS := $(OBJS:.o=.d)
+
+all: boardd
+
+boardd: $(OBJS)
+	@echo "[ LINK ] $@"
+	$(CXX) -fPIC -o '$@' $^ \
+            $(CEREAL_LIBS) \
+            $(ZMQ_LIBS) \
+            $(EXTRA_LIBS)
+
+boardd.o: boardd.cc
+	@echo "[ CXX ] $@"
+	$(CXX) $(CXXFLAGS) \
+           -I$(PHONELIBS)/android_system_core/include \
+           $(CEREAL_FLAGS) \
+           $(ZMQ_FLAGS) \
+           -I../ \
+           -I../../ \
+           -c -o '$@' '$<'
+
+
+log.capnp.o: ../../cereal/gen/cpp/log.capnp.c++
+	@echo "[ CXX ] $@"
+	$(CXX) $(CXXFLAGS) $(CEREAL_FLAGS) \
+           -c -o '$@' '$<'
+  
+
+.PHONY: clean
+clean:
+	rm -f boardd $(OBJS) $(DEPS)
+
+-include $(DEPS)

selfdrive/boardd/boardd.cc

@@ -0,0 +1,322 @@
+#include <stdio.h>
+#include <stdint.h>
+#include <stdlib.h>
+#include <string.h>
+#include <signal.h>
+#include <unistd.h>
+#include <sys/time.h>
+#include <sys/cdefs.h>
+#include <sys/types.h>
+#include <sys/time.h>
+#include <sys/resource.h>
+
+#include <assert.h>
+#include <pthread.h>
+
+#include <zmq.h>
+#include <libusb.h>
+
+#include <capnp/serialize.h>
+#include "cereal/gen/cpp/log.capnp.h"
+
+#include "common/timing.h"
+
+int do_exit = 0;
+
+libusb_context *ctx = NULL;
+libusb_device_handle *dev_handle;
+pthread_mutex_t usb_lock;
+
+// double the FIFO size
+#define RECV_SIZE (0x1000)
+#define TIMEOUT 0
+
+#define DEBUG_BOARDD
+#ifdef DEBUG_BOARDD
+#define DPRINTF(fmt, ...) printf("boardd: " fmt, ## __VA_ARGS__)
+#else
+#define DPRINTF(fmt, ...)
+#endif
+
+bool usb_connect() {
+  int err;
+
+  dev_handle = libusb_open_device_with_vid_pid(ctx, 0xbbaa, 0xddcc);
+  if (dev_handle == NULL) { return false; }
+
+  err = libusb_set_configuration(dev_handle, 1);
+  if (err != 0) { return false; }
+
+  err = libusb_claim_interface(dev_handle, 0);
+  if (err != 0) { return false; }
+
+  return true;
+}
+
+
+void handle_usb_issue(int err, const char func[]) {
+  DPRINTF("usb error %d \"%s\" in %s\n", err, libusb_strerror((enum libusb_error)err), func);
+  if (err == -4) {
+    while (!usb_connect()) { DPRINTF("attempting to connect\n"); usleep(100*1000); }
+  }
+  // TODO: check other errors, is simply retrying okay?
+}
+
+void can_recv(void *s) {
+  int err;
+  uint32_t data[RECV_SIZE/4];
+  int recv;
+  uint32_t f1, f2;
+
+  // 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__); }
+    if (err == -8) { DPRINTF("overflow got 0x%x\n", recv); };
+
+    // timeout is okay to exit, recv still happened
+    if (err == -7) { break; }
+  } while(err != 0);
+
+  pthread_mutex_unlock(&usb_lock);
+
+  // return if length is 0
+  if (recv <= 0) {
+    return;
+  }
+
+  // create message
+  capnp::MallocMessageBuilder msg;
+  cereal::Event::Builder event = msg.initRoot<cereal::Event>();
+  event.setLogMonoTime(nanos_since_boot());
+
+  auto canData = event.initCan(recv/0x10);
+
+  // populate message
+  for (int i = 0; i<(recv/0x10); i++) {
+    if (data[i*4] & 4) {
+      // extended
+      canData[i].setAddress(data[i*4] >> 3);
+      //printf("got extended: %x\n", data[i*4] >> 3);
+    } else {
+      // normal
+      canData[i].setAddress(data[i*4] >> 21);
+    }
+    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) & 3);
+  }
+
+  // send to can
+  auto words = capnp::messageToFlatArray(msg);
+  auto bytes = words.asBytes();
+  zmq_send(s, bytes.begin(), bytes.size(), 0); 
+}
+
+void can_health(void *s) {
+  int cnt;
+
+  // copied from board/main.c
+  struct health {
+    uint32_t voltage;
+    uint32_t current;
+    uint8_t started;
+    uint8_t controls_allowed;
+    uint8_t gas_interceptor_detected;
+  } health;
+
+  // recv from board
+  pthread_mutex_lock(&usb_lock);
+
+  do {
+    cnt = libusb_control_transfer(dev_handle, 0xc0, 0xd2, 0, 0, (unsigned char*)&health, sizeof(health), TIMEOUT);
+    if (cnt != sizeof(health)) { handle_usb_issue(cnt, __func__); }
+  } while(cnt != sizeof(health));
+
+  pthread_mutex_unlock(&usb_lock);
+
+  // create message
+  capnp::MallocMessageBuilder msg;
+  cereal::Event::Builder event = msg.initRoot<cereal::Event>();
+  event.setLogMonoTime(nanos_since_boot());
+  auto healthData = event.initHealth();
+
+  // set fields
+  healthData.setVoltage(health.voltage);
+  healthData.setCurrent(health.current);
+  healthData.setStarted(health.started);
+  healthData.setControlsAllowed(health.controls_allowed);
+  healthData.setGasInterceptorDetected(health.gas_interceptor_detected);
+
+  // send to health
+  auto words = capnp::messageToFlatArray(msg);
+  auto bytes = words.asBytes();
+  zmq_send(s, bytes.begin(), bytes.size(), 0); 
+}
+
+
+void can_send(void *s) {
+  int err;
+
+  // recv from sendcan
+  zmq_msg_t msg;
+  zmq_msg_init(&msg);
+  err = zmq_msg_recv(&msg, s, 0);
+  assert(err >= 0);
+
+  // format for board
+  auto amsg = kj::arrayPtr((const capnp::word*)zmq_msg_data(&msg), zmq_msg_size(&msg));
+  capnp::FlatArrayMessageReader cmsg(amsg);
+  cereal::Event::Reader event = cmsg.getRoot<cereal::Event>();
+  int msg_count = event.getCan().size();
+
+  uint32_t *send = (uint32_t*)malloc(msg_count*0x10);
+  memset(send, 0, msg_count*0x10);
+
+  for (int i = 0; i < msg_count; i++) {
+    auto cmsg = event.getCan()[i];
+    if (cmsg.getAddress() >= 0x800) {
+      // extended
+      send[i*4] = (cmsg.getAddress() << 3) | 5;
+    } else {
+      // normal
+      send[i*4] = (cmsg.getAddress() << 21) | 1;
+    }
+    assert(cmsg.getDat().size() <= 8);
+    send[i*4+1] = cmsg.getDat().size() | (cmsg.getSrc() << 4);
+    memcpy(&send[i*4+2], cmsg.getDat().begin(), cmsg.getDat().size());
+  }
+
+  //DPRINTF("got send message: %d\n", msg_count);
+
+  // release msg
+  zmq_msg_close(&msg);
+
+  // send to board
+  int sent;
+  pthread_mutex_lock(&usb_lock);
+
+  do {
+    err = libusb_bulk_transfer(dev_handle, 3, (uint8_t*)send, msg_count*0x10, &sent, TIMEOUT);
+    if (err != 0 || msg_count*0x10 != sent) { handle_usb_issue(err, __func__); }
+  } while(err != 0);
+
+  pthread_mutex_unlock(&usb_lock);
+
+  // done
+  free(send);
+}
+
+
+// **** threads ****
+
+void *can_send_thread(void *crap) {
+  DPRINTF("start send thread\n");
+
+  // sendcan = 8017
+  void *context = zmq_ctx_new();
+  void *subscriber = zmq_socket(context, ZMQ_SUB);
+  zmq_setsockopt(subscriber, ZMQ_SUBSCRIBE, "", 0);
+  zmq_connect(subscriber, "tcp://127.0.0.1:8017");
+
+  // run as fast as messages come in
+  while (!do_exit) {
+    can_send(subscriber);
+  }
+  return NULL;
+}
+
+void *can_recv_thread(void *crap) {
+  DPRINTF("start recv thread\n");
+
+  // can = 8006
+  void *context = zmq_ctx_new();
+  void *publisher = zmq_socket(context, ZMQ_PUB);
+  zmq_bind(publisher, "tcp://*:8006");
+
+  // run at ~200hz
+  while (!do_exit) {
+    can_recv(publisher);
+    // 5ms
+    usleep(5*1000);
+  }
+  return NULL;
+}
+
+void *can_health_thread(void *crap) {
+  DPRINTF("start health thread\n");
+
+  // health = 8011
+  void *context = zmq_ctx_new();
+  void *publisher = zmq_socket(context, ZMQ_PUB);
+  zmq_bind(publisher, "tcp://*:8011");
+
+  // run at 1hz
+  while (!do_exit) {
+    can_health(publisher);
+    usleep(1000*1000);
+  }
+  return NULL;
+}
+
+int main() {
+  int err;
+  printf("boardd: starting boardd\n");
+
+  // set process priority
+  err = setpriority(PRIO_PROCESS, 0, -4);
+  printf("boardd: setpriority returns %d\n", err);
+
+  // connect to the board
+  err = libusb_init(&ctx);
+  assert(err == 0);
+  libusb_set_debug(ctx, 3);
+
+  // TODO: duplicate code from error handling
+  while (!usb_connect()) { DPRINTF("attempting to connect\n"); usleep(100*1000); }
+
+  /*int config;
+  err = libusb_get_configuration(dev_handle, &config);
+  assert(err == 0);
+  DPRINTF("configuration is %d\n", config);*/
+
+  /*err = libusb_set_interface_alt_setting(dev_handle, 0, 0);
+  assert(err == 0);*/
+
+  // create threads
+
+  pthread_t can_health_thread_handle;
+  err = pthread_create(&can_health_thread_handle, NULL,
+                       can_health_thread, NULL);
+  assert(err == 0);
+
+  pthread_t can_send_thread_handle;
+  err = pthread_create(&can_send_thread_handle, NULL,
+                       can_send_thread, NULL);
+  assert(err == 0);
+
+  pthread_t can_recv_thread_handle;
+  err = pthread_create(&can_recv_thread_handle, NULL,
+                       can_recv_thread, NULL);
+  assert(err == 0);
+
+  // join threads
+
+  err = pthread_join(can_recv_thread_handle, NULL);
+  assert(err == 0);
+
+  err = pthread_join(can_send_thread_handle, NULL);
+  assert(err == 0);
+
+  err = pthread_join(can_health_thread_handle, NULL);
+  assert(err == 0);
+
+  // destruct libusb
+
+  libusb_close(dev_handle);
+  libusb_exit(ctx);
+}
+

selfdrive/boardd/boardd.py

@@ -0,0 +1,179 @@
+#!/usr/bin/env python
+import os
+import struct
+import zmq
+
+import selfdrive.messaging as messaging
+from common.realtime import Ratekeeper
+from common.services import service_list
+from selfdrive.swaglog import cloudlog
+
+# USB is optional
+try:
+  import usb1
+  from usb1 import USBErrorIO, USBErrorOverflow
+except Exception:
+  pass
+
+# TODO: rewrite in C to save CPU
+
+# *** serialization functions ***
+def can_list_to_can_capnp(can_msgs):
+  dat = messaging.new_message()
+  dat.init('can', len(can_msgs))
+  for i, can_msg in enumerate(can_msgs):
+    dat.can[i].address = can_msg[0]
+    dat.can[i].busTime = can_msg[1]
+    dat.can[i].dat = can_msg[2]
+    dat.can[i].src = can_msg[3]
+  return dat
+
+def can_capnp_to_can_list_old(dat, src_filter=[]):
+  ret = []
+  for msg in dat.can:
+    if msg.src in src_filter:
+      ret.append([msg.address, msg.busTime, msg.dat.encode("hex")])
+  return ret
+
+def can_capnp_to_can_list(dat):
+  ret = []
+  for msg in dat.can:
+    ret.append([msg.address, msg.busTime, msg.dat, msg.src])
+  return ret
+
+# *** can driver ***
+def can_health():
+  while 1:
+    try:
+      dat = handle.controlRead(usb1.TYPE_VENDOR | usb1.RECIPIENT_DEVICE, 0xd2, 0, 0, 0x10)
+      break
+    except (USBErrorIO, USBErrorOverflow):
+      cloudlog.exception("CAN: BAD HEALTH, RETRYING")
+  v, i, started = struct.unpack("IIB", dat[0:9])
+  # TODO: units
+  return {"voltage": v, "current": i, "started": bool(started)}
+
+def __parse_can_buffer(dat):
+  ret = []
+  for j in range(0, len(dat), 0x10):
+    ddat = dat[j:j+0x10]
+    f1, f2 = struct.unpack("II", ddat[0:8])
+    ret.append((f1 >> 21, f2>>16, ddat[8:8+(f2&0xF)], (f2>>4)&3))
+  return ret
+
+def can_send_many(arr):
+  snds = []
+  for addr, _, dat, alt in arr:
+    snd = struct.pack("II", ((addr << 21) | 1), len(dat) | (alt << 4)) + dat
+    snd = snd.ljust(0x10, '\x00')
+    snds.append(snd)
+  while 1:
+    try:
+      handle.bulkWrite(3, ''.join(snds))
+      break
+    except (USBErrorIO, USBErrorOverflow):
+      cloudlog.exception("CAN: BAD SEND MANY, RETRYING")
+
+def can_recv():
+  dat = ""
+  while 1:
+    try:
+      dat = handle.bulkRead(1, 0x10*256)
+      break
+    except (USBErrorIO, USBErrorOverflow):
+      cloudlog.exception("CAN: BAD RECV, RETRYING")
+  return __parse_can_buffer(dat)
+
+def can_init():
+  global handle, context
+  cloudlog.info("attempting can init")
+
+  context = usb1.USBContext()
+  #context.setDebug(9)
+
+  for device in context.getDeviceList(skip_on_error=True):
+    if device.getVendorID() == 0xbbaa and device.getProductID() == 0xddcc:
+      handle = device.open()
+      handle.claimInterface(0)
+
+  if handle is None:
+    print "CAN NOT FOUND"
+    exit(-1)
+
+  print "got handle"
+  cloudlog.info("can init done")
+
+def boardd_mock_loop():
+  context = zmq.Context()
+  can_init()
+
+  logcan = messaging.sub_sock(context, service_list['can'].port)
+
+  while 1:
+    tsc = messaging.drain_sock(logcan, wait_for_one=True)
+    snds = map(can_capnp_to_can_list, tsc)
+    snd = []
+    for s in snds:
+      snd += s
+    snd = filter(lambda x: x[-1] <= 1, snd)
+    can_send_many(snd)
+
+    # recv @ 100hz
+    can_msgs = can_recv()
+    print "sent %d got %d" % (len(snd), len(can_msgs))
+
+    #print can_msgs
+
+# *** main loop ***
+def boardd_loop(rate=200):
+  rk = Ratekeeper(rate)
+  context = zmq.Context()
+
+  can_init()
+
+  # *** publishes can and health
+  logcan = messaging.pub_sock(context, service_list['can'].port)
+  health_sock = messaging.pub_sock(context, service_list['health'].port)
+
+  # *** subscribes to can send
+  sendcan = messaging.sub_sock(context, service_list['sendcan'].port)
+
+  while 1:
+    # health packet @ 1hz
+    if (rk.frame%rate) == 0:
+      health = can_health()
+      msg = messaging.new_message()
+      msg.init('health')
+
+      # store the health to be logged
+      msg.health.voltage = health['voltage']
+      msg.health.current = health['current']
+      msg.health.started = health['started']
+
+      health_sock.send(msg.to_bytes())
+
+    # recv @ 100hz
+    can_msgs = can_recv()
+
+    # publish to logger
+    # TODO: refactor for speed
+    if len(can_msgs) > 0:
+      dat = can_list_to_can_capnp(can_msgs)
+      logcan.send(dat.to_bytes())
+
+    # send can if we have a packet
+    tsc = messaging.recv_sock(sendcan)
+    if tsc is not None:
+      can_send_many(can_capnp_to_can_list(tsc))
+
+    rk.keep_time()
+
+def main(gctx=None):
+  if os.getenv("MOCK") is not None:
+    boardd_mock_loop()
+  else:
+    boardd_loop()
+
+if __name__ == "__main__":
+  main()
+

selfdrive/calibrationd/calibration.py

@@ -0,0 +1,208 @@
+import numpy as np
+
+import common.filters as filters
+from selfdrive.controls.lib.latcontrol import calc_curvature
+
+
+# Calibration Status
+class CalibStatus(object):
+  INCOMPLETE = 0
+  VALID = 1
+  INVALID = 2
+
+
+def line_intersection(line1, line2, no_int_sub = [0,0]):
+  xdiff = (line1[0][0] - line1[1][0], line2[0][0] - line2[1][0])
+  ydiff = (line1[0][1] - line1[1][1], line2[0][1] - line2[1][1])
+
+  def det(a, b):
+    return a[0] * b[1] - a[1] * b[0]
+
+  div = det(xdiff, ydiff)
+  if div == 0:
+    # since we are in float domain, this should really never happen
+    return no_int_sub
+
+  d = (det(*line1), det(*line2))
+  x = det(d, xdiff) / div
+  y = det(d, ydiff) / div
+  return [x, y]
+
+def points_inside_hit_box(pts, box):
+  """Determine which points lie inside a box.
+
+     Inputs:
+      pts: An nx2 array of points to hit test.
+      box: An array [[x_left, y_top], [x_right, y_bottom]] describing a box to
+           use for hit testing.
+     Returns:
+      A logical array with true for every member of pts inside box.
+  """
+  hits = np.all(np.logical_and(pts > box[0, :], pts < box[1, :]), axis=1)
+  return hits
+
+def warp_points(pt_s, warp_matrix):
+  # pt_s are the source points, nx2 array.
+  pt_d = np.dot(warp_matrix[:, :2], pt_s.T) + warp_matrix[:, 2][:, np.newaxis]
+
+  # divide by third dimension for representation in image space.
+  return (pt_d[:2, :] / pt_d[2, :]).T
+
+class ViewCalibrator(object):
+  def __init__(self, box_size, big_box_size, vp_r, warp_matrix_start, vp_f=None, cal_cycle=0, cal_status=0):
+    self.calibration_threshold = 3000
+    self.box_size = box_size
+    self.big_box_size = big_box_size
+
+    self.warp_matrix_start = warp_matrix_start
+    self.vp_r = list(vp_r)
+
+    if vp_f is None:
+      self.vp_f = list(vp_r)
+    else:
+      self.vp_f = list(vp_f)
+
+    # slow filter fot the vanishing point 
+    vp_fr = 0.005    # Hz, slow filter 
+    self.dt = 0.05  # camera runs at 20Hz
+
+    self.update_warp_matrix()
+
+    self.vp_x_filter = filters.FirstOrderLowpassFilter(vp_fr, self.dt, self.vp_f[0])
+    self.vp_y_filter = filters.FirstOrderLowpassFilter(vp_fr, self.dt, self.vp_f[1])
+
+    self.cal_cycle = cal_cycle
+    self.cal_status = cal_status
+    self.cal_perc = int(np.minimum(self.cal_cycle*100./self.calibration_threshold, 100))
+
+  def vanishing_point_process(self, old_ps, new_ps, v_ego, steer_angle, VP):
+    # correct diffs by yaw rate 
+    cam_fov = 23.06*np.pi/180. # deg
+    curvature = calc_curvature(v_ego, steer_angle, VP)
+    yaw_rate = curvature * v_ego
+    hor_angle_shift = yaw_rate * self.dt * self.box_size[0] / cam_fov
+    old_ps += [hor_angle_shift, 0]  # old points have moved in the image due to yaw rate
+
+    pos_ps = [None]*len(new_ps)
+    for ii in range(len(old_ps)):
+      xo = old_ps[ii][0]
+      yo = old_ps[ii][1]
+      yn = new_ps[ii][1]
+
+      # don't consider points with low flow in y
+      if abs(yn - yo) > 1:
+        if xo > (self.vp_f[0] + 20):
+          pos_ps[ii] = 'r'   # right lane point
+        elif xo < (self.vp_f[0] - 20):
+          pos_ps[ii] = 'l'   # left lane point
+
+        # intersect all the right lines with the left lines
+    idxs_l = [i for i, x in enumerate(pos_ps) if x == 'l']
+    idxs_r = [i for i, x in enumerate(pos_ps) if x == 'r']
+
+    old_ps_l, new_ps_l = old_ps[idxs_l], new_ps[idxs_l]
+    old_ps_r, new_ps_r = old_ps[idxs_r], new_ps[idxs_r]
+    # return None if there is one side with no lines, the speed is low or the steer angle is high 
+    if len(old_ps_l) == 0 or len(old_ps_r) == 0 or v_ego < 20 or abs(steer_angle) > 5:
+      return None
+
+    int_ps = [[None] * len(old_ps_r)] * len(old_ps_l)
+    for ll in range(len(old_ps_l)):
+      for rr in range(len(old_ps_r)):
+        old_p_l, old_p_r, new_p_l, new_p_r = old_ps_l[ll], old_ps_r[
+          rr], new_ps_l[ll], new_ps_r[rr]
+        line_l = [[old_p_l[0], old_p_l[1]], [new_p_l[0], new_p_l[1]]]
+        line_r = [[old_p_r[0], old_p_r[1]], [new_p_r[0], new_p_r[1]]]
+        int_ps[ll][rr] = line_intersection(
+          line_l, line_r, no_int_sub=self.vp_f)
+        # saturate outliers that are too far from the estimated vp
+        int_ps[ll][rr][0] = np.clip(int_ps[ll][rr][0], self.vp_f[0] - 20, self.vp_f[0] + 20)
+        int_ps[ll][rr][1] = np.clip(int_ps[ll][rr][1], self.vp_f[1] - 30, self.vp_f[1] + 30)
+    vp = np.mean(np.mean(np.array(int_ps), axis=0), axis=0)
+
+    return vp
+
+  def calibration_validity(self):
+    # this function sanity checks that the small box is contained in the big box.
+    # otherwise the warp function will generate black spots on the small box
+    cp = np.asarray([[0, 0],
+                    [self.box_size[0], 0],
+                    [self.box_size[0], self.box_size[1]],
+                    [0, self.box_size[1]]])
+
+    cpw = warp_points(cp, self.warp_matrix)
+
+    # pixel margin for validity hysteresys: 
+    # - if calibration is good, keep it good until small box is inside the big box
+    # - if calibration isn't good, then make it good again if small box is in big box with margin
+    margin_px = 0 if self.cal_status == CalibStatus.VALID else 5
+    big_hit_box = np.asarray(
+      [[margin_px, margin_px],
+       [self.big_box_size[0], self.big_box_size[1] - margin_px]])
+
+    cpw_outside_big_box = np.logical_not(points_inside_hit_box(cpw, big_hit_box))
+    return not np.any(cpw_outside_big_box)
+
+
+  def get_calibration_hit_box(self):
+    """Returns an axis-aligned hit box in canonical image space.
+       Points which do not fall within this box should not be used for
+       calibration.
+
+       Returns:
+        An array [[x_left, y_top], [x_right, y_bottom]] describing a box inside
+        which all calibration points should lie.
+    """
+    # We mainly care about feature from lanes, so removed points from sky.
+    y_filter = 50.
+    return np.asarray([[0, y_filter], [self.box_size[0], self.box_size[1]]])
+
+
+  def update_warp_matrix(self):
+    translation_matrix = np.asarray(
+      [[1, 0, self.vp_f[0] - self.vp_r[0]],
+       [0, 1, self.vp_f[1] - self.vp_r[1]],
+       [0, 0, 1]])
+    self.warp_matrix = np.dot(translation_matrix, self.warp_matrix_start)
+    self.warp_matrix_inv = np.linalg.inv(self.warp_matrix)
+
+  def calibration(self, p0, p1, st, v_ego, steer_angle, VP):
+    # convert to np array first thing
+    p0 = np.asarray(p0)
+    p1 = np.asarray(p1)
+    st = np.asarray(st)
+
+    p0 = p0.reshape((-1,2))
+    p1 = p1.reshape((-1,2))
+
+    # filter out pts with bad status
+    p0 = p0[st==1]
+    p1 = p1[st==1]
+
+    calib_hit_box = self.get_calibration_hit_box()
+    # remove all the points outside the small box and above the horizon line
+    good_idxs = points_inside_hit_box(
+      warp_points(p0, self.warp_matrix_inv), calib_hit_box)
+    p0 = p0[good_idxs]
+    p1 = p1[good_idxs]
+
+    # print("unwarped points: {}".format(warp_points(p0, self.warp_matrix_inv)))
+    # print("good_idxs {}:".format(good_idxs))
+
+    # get instantaneous vp
+    vp = self.vanishing_point_process(p0, p1, v_ego, steer_angle, VP)
+
+    if vp is not None:
+      # filter the vanishing point
+      self.vp_f = [self.vp_x_filter(vp[0]), self.vp_y_filter(vp[1])]
+      self.cal_cycle += 1
+
+    if not self.calibration_validity():
+      self.cal_status = CalibStatus.INVALID
+    else:
+      # 10 minutes @5Hz TODO: make this threshold function of convergency speed
+      self.cal_status = CalibStatus.VALID
+      #self.cal_status = CalibStatus.VALID if self.cal_cycle > self.calibration_threshold else CalibStatus.INCOMPLETE
+    self.cal_perc = int(np.minimum(self.cal_cycle*100./self.calibration_threshold, 100))
+
+    self.update_warp_matrix()

selfdrive/calibrationd/calibrationd.py

@@ -0,0 +1,116 @@
+#!/usr/bin/env python
+import os
+import numpy as np
+import zmq
+
+from common.services import service_list
+import selfdrive.messaging as messaging
+from selfdrive.config import ImageParams, VehicleParams
+from selfdrive.calibrationd.calibration import ViewCalibrator, CalibStatus
+
+CALIBRATION_FILE = "/sdcard/calibration_param"
+
+def load_calibration(gctx):
+  # calibration initialization
+  I = ImageParams()
+  vp_guess = None
+
+  if gctx is not None:
+    warp_matrix_start = np.array(
+        gctx['calibration']["initial_homography"]).reshape(3, 3)
+    big_box_size = [560, 304]
+  else:
+    warp_matrix_start = np.array([[1., 0., I.SX_R],
+                                  [0., 1., I.SY_R],
+                                  [0., 0., 1.]])
+    big_box_size = [640, 480]
+
+  # translate the vanishing point into phone image space
+  vp_box = (I.VPX_R-I.SX_R, I.VPY_R-I.SY_R)
+  vp_trans = np.dot(warp_matrix_start, vp_box+(1.,))
+  vp_img = (vp_trans[0]/vp_trans[2], vp_trans[1]/vp_trans[2])
+
+  # load calibration data
+  if os.path.isfile(CALIBRATION_FILE):
+    # if the calibration file exist, start from the last cal values
+    with open(CALIBRATION_FILE, "r") as cal_file:
+      data = [float(l.strip()) for l in cal_file.readlines()]
+      calib = ViewCalibrator((I.X, I.Y),
+                             big_box_size,
+                             vp_img,
+                             warp_matrix_start,
+                             vp_f=[data[2], data[3]],
+                             cal_cycle=data[0],
+                             cal_status=data[1])
+
+      if calib.cal_status == CalibStatus.INCOMPLETE:
+        print "CALIBRATION IN PROGRESS", calib.cal_cycle
+  else:
+    print "NO CALIBRATION FILE"
+    calib = ViewCalibrator((I.X, I.Y),
+                           big_box_size,
+                           vp_img,
+                           warp_matrix_start,
+                           vp_f=vp_guess)
+
+  return calib
+
+def calibrationd_thread(gctx):
+  context = zmq.Context()
+
+  features = messaging.sub_sock(context, service_list['features'].port)
+  live100 = messaging.sub_sock(context, service_list['live100'].port)
+
+  livecalibration = messaging.pub_sock(context, service_list['liveCalibration'].port)
+
+  # subscribe to stats about the car
+  VP = VehicleParams(False)
+
+  v_ego = None
+
+  calib = load_calibration(gctx)
+  last_cal_cycle = calib.cal_cycle
+
+  while 1:
+    # calibration at the end so it does not delay radar processing above
+    ft = messaging.recv_sock(features, wait=True)
+
+    # get latest here
+    l100 = messaging.recv_sock(live100)
+    if l100 is not None:
+      v_ego = l100.live100.vEgo
+      steer_angle = l100.live100.angleSteers
+
+    if v_ego is None:
+      continue
+
+    p0 = ft.features.p0
+    p1 = ft.features.p1
+    st = ft.features.status
+
+    calib.calibration(p0, p1, st, v_ego, steer_angle, VP)
+
+    # write a new calibration every 100 cal cycle
+    if calib.cal_cycle - last_cal_cycle >= 100:
+      print "writing cal", calib.cal_cycle
+      with open(CALIBRATION_FILE, "w") as cal_file:
+        cal_file.write(str(calib.cal_cycle)+'\n')
+        cal_file.write(str(calib.cal_status)+'\n')
+        cal_file.write(str(calib.vp_f[0])+'\n')
+        cal_file.write(str(calib.vp_f[1])+'\n')
+      last_cal_cycle = calib.cal_cycle
+
+    warp_matrix = map(float, calib.warp_matrix.reshape(9).tolist())
+    dat = messaging.new_message()
+    dat.init('liveCalibration')
+    dat.liveCalibration.warpMatrix = warp_matrix
+    dat.liveCalibration.calStatus = calib.cal_status
+    dat.liveCalibration.calCycle = calib.cal_cycle
+    dat.liveCalibration.calPerc = calib.cal_perc
+    livecalibration.send(dat.to_bytes())
+
+def main(gctx=None):
+  calibrationd_thread(gctx)
+
+if __name__ == "__main__":
+  main()

selfdrive/common/framebuffer.cc

@@ -0,0 +1,135 @@
+
+#include <cstdio>
+#include <cstdlib>
+#include <cassert>
+
+#include <ui/DisplayInfo.h>
+
+#include <gui/ISurfaceComposer.h>
+#include <gui/Surface.h>
+#include <gui/SurfaceComposerClient.h>
+
+
+#include <GLES2/gl2.h>
+#include <EGL/eglext.h>
+
+#define BACKLIGHT_CONTROL "/sys/class/leds/lcd-backlight/brightness"
+#define BACKLIGHT_LEVEL "205"
+
+using namespace android;
+
+struct FramebufferState {
+    sp<SurfaceComposerClient> session;
+    sp<IBinder> dtoken;
+    DisplayInfo dinfo;
+    sp<SurfaceControl> control;
+
+    sp<Surface> s;
+    EGLDisplay display;
+
+    EGLint egl_major, egl_minor;
+    EGLConfig config;
+    EGLSurface surface;
+    EGLContext context;
+};
+
+extern "C" FramebufferState* framebuffer_init(
+    const char* name, int32_t layer,
+    EGLDisplay *out_display, EGLSurface *out_surface,
+    int *out_w, int *out_h) {
+  status_t status;
+  int success;
+
+  FramebufferState *s = new FramebufferState;
+
+  s->session = new SurfaceComposerClient();
+  assert(s->session != NULL);
+
+  s->dtoken = SurfaceComposerClient::getBuiltInDisplay(
+                ISurfaceComposer::eDisplayIdMain);
+  assert(s->dtoken != NULL);
+
+  status = SurfaceComposerClient::getDisplayInfo(s->dtoken, &s->dinfo);
+  assert(status == 0);
+
+  int orientation = 3; // rotate framebuffer 270 degrees
+  if(orientation == 1 || orientation == 3) {
+      int temp = s->dinfo.h;
+      s->dinfo.h = s->dinfo.w;
+      s->dinfo.w = temp;
+  }
+
+  printf("dinfo %dx%d\n", s->dinfo.w, s->dinfo.h);
+
+  Rect destRect(s->dinfo.w, s->dinfo.h);
+  s->session->setDisplayProjection(s->dtoken, orientation, destRect, destRect);
+
+  s->control = s->session->createSurface(String8(name),
+                  s->dinfo.w, s->dinfo.h, PIXEL_FORMAT_RGBX_8888);
+  assert(s->control != NULL);
+
+  SurfaceComposerClient::openGlobalTransaction();
+  status = s->control->setLayer(layer);
+  SurfaceComposerClient::closeGlobalTransaction();
+  assert(status == 0);
+
+  s->s = s->control->getSurface();
+  assert(s->s != NULL);
+
+  // init opengl and egl
+  const EGLint attribs[] = {
+    EGL_RED_SIZE,     8,
+    EGL_GREEN_SIZE,   8,
+    EGL_BLUE_SIZE,    8,
+    EGL_DEPTH_SIZE,   0,
+    EGL_STENCIL_SIZE, 8,
+    EGL_RENDERABLE_TYPE, EGL_OPENGL_ES3_BIT_KHR,
+    EGL_NONE,
+  };
+
+  s->display = eglGetDisplay(EGL_DEFAULT_DISPLAY);
+  assert(s->display != EGL_NO_DISPLAY);
+
+  success = eglInitialize(s->display, &s->egl_major, &s->egl_minor);
+  assert(success);
+
+  printf("egl version %d.%d\n", s->egl_major, s->egl_minor);
+  
+  EGLint num_configs;
+  success = eglChooseConfig(s->display, attribs, &s->config, 1, &num_configs);
+  assert(success);
+
+  s->surface = eglCreateWindowSurface(s->display, s->config, s->s.get(), NULL);
+  assert(s->surface != EGL_NO_SURFACE);
+
+  const EGLint context_attribs[] = {
+    EGL_CONTEXT_CLIENT_VERSION, 3,
+    EGL_NONE,
+  };
+  s->context = eglCreateContext(s->display, s->config, NULL, context_attribs);
+  assert(s->context != EGL_NO_CONTEXT);
+
+  EGLint w, h;
+  eglQuerySurface(s->display, s->surface, EGL_WIDTH, &w);
+  eglQuerySurface(s->display, s->surface, EGL_HEIGHT, &h);
+  printf("egl w %d h %d\n", w, h);
+
+  success = eglMakeCurrent(s->display, s->surface, s->surface, s->context);
+  assert(success);
+
+  printf("gl version %s\n", glGetString(GL_VERSION));
+
+
+  // set brightness
+  int brightness_fd = open(BACKLIGHT_CONTROL, O_RDWR);
+  const char brightness_level[] = BACKLIGHT_LEVEL;
+  write(brightness_fd, brightness_level, strlen(brightness_level));
+
+
+  if (out_display) *out_display = s->display;
+  if (out_surface) *out_surface = s->surface;
+  if (out_w) *out_w = w;
+  if (out_h) *out_h = h;
+
+  return s;
+}

selfdrive/common/framebuffer.h

@@ -0,0 +1,21 @@
+#ifndef FRAMEBUFFER_H
+#define FRAMEBUFFER_H
+
+#include <EGL/eglext.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+typedef struct FramebufferState FramebufferState;
+
+FramebufferState* framebuffer_init(
+    const char* name, int32_t layer,
+    EGLDisplay *out_display, EGLSurface *out_surface,
+    int *out_w, int *out_h);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif

selfdrive/common/mat.h

@@ -0,0 +1,68 @@
+#ifndef COMMON_MAT_H
+#define COMMON_MAT_H
+
+typedef struct vec3 {
+	float v[3];
+} vec3;
+
+typedef struct vec4 {
+  float v[4];
+} vec4;
+
+typedef struct mat3 {
+	float v[3*3];
+} mat3;
+
+typedef struct mat4 {
+  float v[4*4];
+} mat4;
+
+static inline mat3 matmul3(const mat3 a, const mat3 b) {
+  mat3 ret = {{0.0}};
+  for (int r=0; r<3; r++) {
+    for (int c=0; c<3; c++) {
+      float v = 0.0;
+      for (int k=0; k<3; k++) {
+        v += a.v[r*3+k] * b.v[k*3+c];
+      }
+      ret.v[r*3+c] = v;
+    }
+  }
+  return ret;
+}
+
+static inline vec3 matvecmul3(const mat3 a, const vec3 b) {
+  vec3 ret = {{0.0}};
+  for (int r=0; r<3; r++) {
+    for (int c=0; c<3; c++) {
+      ret.v[r] += a.v[r*3+c] * b.v[c];
+    }
+  }
+  return ret;
+}
+
+static inline mat4 matmul(const mat4 a, const mat4 b) {
+  mat4 ret = {{0.0}};
+  for (int r=0; r<4; r++) {
+    for (int c=0; c<4; c++) {
+      float v = 0.0;
+      for (int k=0; k<4; k++) {
+        v += a.v[r*4+k] * b.v[k*4+c];
+      }
+      ret.v[r*4+c] = v;
+    }
+  }
+  return ret;
+}
+
+static inline vec4 matvecmul(const mat4 a, const vec4 b) {
+  vec4 ret = {{0.0}};
+  for (int r=0; r<4; r++) {
+    for (int c=0; c<4; c++) {
+      ret.v[r] += a.v[r*4+c] * b.v[c];
+    }
+  }
+  return ret;
+}
+
+#endif

selfdrive/common/modeldata.h

@@ -0,0 +1,23 @@
+#ifndef MODELDATA_H
+#define MODELDATA_H
+
+typedef struct PathData {
+  float points[50];
+  float prob;
+  float std;
+} PathData;
+
+typedef struct LeadData {
+  float dist;
+  float prob;
+  float std;
+} LeadData;
+
+typedef struct ModelData {
+  PathData path;
+  PathData left_lane;
+  PathData right_lane;
+  LeadData lead;
+} ModelData;
+
+#endif

selfdrive/common/mutex.h

@@ -0,0 +1,13 @@
+#ifndef COMMON_MUTEX_H
+#define COMMON_MUTEX_H
+
+#include <pthread.h>
+
+static inline void mutex_init_reentrant(pthread_mutex_t *mutex) {
+  pthread_mutexattr_t attr;
+  pthread_mutexattr_init(&attr);
+  pthread_mutexattr_settype(&attr, PTHREAD_MUTEX_RECURSIVE);
+  pthread_mutex_init(mutex, &attr);
+}
+
+#endif

selfdrive/common/swaglog.c

@@ -0,0 +1,90 @@
+#include <stdio.h>
+#include <stdlib.h>
+#include <stdarg.h>
+#include <stdbool.h>
+#include <string.h>
+#include <assert.h>
+
+#include <pthread.h>
+#include <zmq.h>
+#include <json.h>
+
+#include "common/timing.h"
+
+#include "swaglog.h"
+
+typedef struct LogState {
+  pthread_mutex_t lock;
+  bool inited;
+  JsonNode *ctx_j;
+  void *zctx;
+  void *sock;
+} LogState;
+
+static LogState s = {
+  .lock = PTHREAD_MUTEX_INITIALIZER,
+};
+
+static void cloudlog_init() {
+  if (s.inited) return;
+  s.ctx_j = json_mkobject();
+  s.zctx = zmq_ctx_new();
+  s.sock = zmq_socket(s.zctx, ZMQ_PUSH);
+  zmq_connect(s.sock, "ipc:///tmp/logmessage");
+  s.inited = true;
+}
+
+void cloudlog_e(int levelnum, const char* filename, int lineno, const char* func, const char* srctime, 
+                const char* fmt, ...) {
+  pthread_mutex_lock(&s.lock);
+  cloudlog_init();
+
+  char* msg_buf = NULL;
+  va_list args;
+  va_start(args, fmt);
+  vasprintf(&msg_buf, fmt, args);
+  va_end(args);
+
+  if (!msg_buf) {
+    pthread_mutex_unlock(&s.lock);
+    return;
+  }
+
+  if (levelnum >= CLOUDLOG_PRINT_LEVEL) {
+    printf("%s: %s\n", filename, msg_buf);
+  }
+
+  JsonNode *log_j = json_mkobject();
+  assert(log_j);
+
+  json_append_member(log_j, "msg", json_mkstring(msg_buf));
+  json_append_member(log_j, "ctx", s.ctx_j);
+  json_append_member(log_j, "levelnum", json_mknumber(levelnum));
+  json_append_member(log_j, "filename", json_mkstring(filename));
+  json_append_member(log_j, "lineno", json_mknumber(lineno));
+  json_append_member(log_j, "funcname", json_mkstring(func));
+  json_append_member(log_j, "srctime", json_mkstring(srctime));
+  json_append_member(log_j, "created", json_mknumber(seconds_since_epoch()));
+
+  char* log_s = json_encode(log_j);
+  assert(log_s);
+
+  json_remove_from_parent(s.ctx_j);  
+
+  json_delete(log_j);
+  free(msg_buf);
+
+  char levelnum_c = levelnum;
+  zmq_send(s.sock, &levelnum_c, 1, ZMQ_NOBLOCK | ZMQ_SNDMORE);
+  zmq_send(s.sock, log_s, strlen(log_s), ZMQ_NOBLOCK);
+  free(log_s);
+
+  pthread_mutex_unlock(&s.lock);
+}
+
+void cloudlog_bind(const char* k, const char* v) {
+  pthread_mutex_lock(&s.lock);
+  cloudlog_init();
+  json_append_member(s.ctx_j, k, json_mkstring(v));
+  pthread_mutex_unlock(&s.lock);
+}

selfdrive/common/swaglog.h

@@ -0,0 +1,26 @@
+#ifndef SWAGLOG_H
+#define SWAGLOG_H
+
+#define CLOUDLOG_DEBUG 10
+#define CLOUDLOG_INFO 20
+#define CLOUDLOG_WARNING 30
+#define CLOUDLOG_ERROR 40
+#define CLOUDLOG_CRITICAL 50
+
+#define CLOUDLOG_PRINT_LEVEL CLOUDLOG_WARNING
+
+void cloudlog_e(int levelnum, const char* filename, int lineno, const char* func, const char* srctime, 
+                const char* fmt, ...) /*__attribute__ ((format (printf, 6, 7)))*/;
+
+void cloudlog_bind(const char* k, const char* v);
+
+#define cloudlog(lvl, fmt, ...) cloudlog_e(lvl, __FILE__, __LINE__, \
+                                           __func__, __DATE__ " " __TIME__, \
+                                           fmt, ## __VA_ARGS__)
+
+#define LOGD(fmt, ...) cloudlog(CLOUDLOG_DEBUG, fmt, ## __VA_ARGS__)
+#define LOG(fmt, ...) cloudlog(CLOUDLOG_INFO, fmt, ## __VA_ARGS__)
+#define LOGW(fmt, ...) cloudlog(CLOUDLOG_WARNING, fmt, ## __VA_ARGS__)
+#define LOGE(fmt, ...) cloudlog(CLOUDLOG_ERROR, fmt, ## __VA_ARGS__)
+
+#endif

selfdrive/common/timing.h

@@ -0,0 +1,31 @@
+#ifndef COMMON_TIMING_H
+#define COMMON_TIMING_H
+
+#include <stdint.h>
+#include <time.h>
+
+static inline uint64_t nanos_since_boot() {
+  struct timespec t;
+  clock_gettime(CLOCK_BOOTTIME, &t);
+  return t.tv_sec * 1000000000ULL + t.tv_nsec;
+}
+
+static inline double millis_since_boot() {
+  struct timespec t;
+  clock_gettime(CLOCK_BOOTTIME, &t);
+  return t.tv_sec * 1000.0 + t.tv_nsec / 1000000.0;
+}
+
+static inline uint64_t nanos_since_epoch() {
+  struct timespec t;
+  clock_gettime(CLOCK_REALTIME, &t);
+  return t.tv_sec * 1000000000ULL + t.tv_nsec;
+}
+
+static inline double seconds_since_epoch() {
+  struct timespec t;
+  clock_gettime(CLOCK_REALTIME, &t);
+  return (double)t.tv_sec + t.tv_nsec / 1000000000.0;
+}
+
+#endif

selfdrive/common/util.h

@@ -0,0 +1,22 @@
+#ifndef COMMON_UTIL_H
+#define COMMON_UTIL_H
+
+#define min(a,b) \
+ ({ __typeof__ (a) _a = (a); \
+     __typeof__ (b) _b = (b); \
+   _a < _b ? _a : _b; })
+
+#define max(a,b) \
+ ({ __typeof__ (a) _a = (a); \
+     __typeof__ (b) _b = (b); \
+   _a > _b ? _a : _b; })
+
+#define clamp(a,b,c) \
+ ({ __typeof__ (a) _a = (a); \
+     __typeof__ (b) _b = (b); \
+     __typeof__ (c) _c = (c); \
+   _a < _b ? _b : (_a > _c ? _c : _a); })
+
+#define ARRAYSIZE(x) (sizeof(x)/sizeof(x[0]))
+
+#endif

selfdrive/common/visionipc.c

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

selfdrive/common/visionipc.h

@@ -0,0 +1,50 @@
+#ifndef VISIONIPC_H
+#define VISIONIPC_H
+
+#define VIPC_SOCKET_PATH "/tmp/vision_socket"
+#define VIPC_MAX_FDS 64
+
+
+#define VISION_INVALID 0
+#define VISION_UI_SUBSCRIBE 1
+#define VISION_UI_BUFS 2
+#define VISION_UI_ACQUIRE 3
+#define VISION_UI_RELEASE 4
+
+typedef struct VisionUIBufs {
+  int width, height, stride;
+  int front_width, front_height, front_stride;
+
+  int big_box_x, big_box_y;
+  int big_box_width, big_box_height;
+  int transformed_width, transformed_height;
+
+  int front_box_x, front_box_y;
+  int front_box_width, front_box_height;
+
+  size_t buf_len;
+  int num_bufs;
+  size_t front_buf_len;
+  int num_front_bufs;
+} VisionUIBufs;
+
+typedef union VisionPacketData {
+  VisionUIBufs ui_bufs;
+  struct {
+    bool front;
+    int idx;
+  } ui_acq, ui_rel;
+} VisionPacketData;
+
+typedef struct VisionPacket {
+  int type;
+  VisionPacketData d;
+  int num_fds;
+  int fds[VIPC_MAX_FDS];
+} VisionPacket;
+
+int vipc_connect();
+int vipc_recv(int fd, VisionPacket *out_p);
+int vipc_send(int fd, const VisionPacket p);
+
+#endif

selfdrive/config.py

@@ -0,0 +1,68 @@
+import numpy as np
+
+class Conversions:
+  MPH_TO_MS = 1.609/3.6
+  MS_TO_MPH = 3.6/1.609
+  KPH_TO_MS = 1./3.6
+  MS_TO_KPH = 3.6
+  MPH_TO_KPH = 1.609
+  KPH_TO_MPH = 1./1.609
+  KNOTS_TO_MS = 1/1.9438
+  MS_TO_KNOTS = 1.9438
+
+  # Car tecode decimal minutes into decimal degrees, can work with numpy arrays as input
+  @staticmethod
+  def dm2d(dm):
+    degs = np.round(dm/100.)
+    mins = dm - degs*100.
+    return degs + mins/60.
+
+
+# Car button codes
+class CruiseButtons:
+  RES_ACCEL   = 4
+  DECEL_SET   = 3
+  CANCEL      = 2
+  MAIN        = 1
+
+
+# Image params for color cam on acura, calibrated on pre las vegas drive (2016-05-21)
+class ImageParams:
+  def __init__(self):
+    self.SX_R  = 160  # top left corner pixel shift of the visual region considered by the model
+    self.SY_R  = 180  # top left corner pixel shift of the visual region considered by the model
+    self.VPX_R = 319  # vanishing point reference, as calibrated in Vegas drive 
+    self.VPY_R = 201  # vanishing point reference, as calibrated in Vegas drive 
+    self.X     = 320  # pixel length of image for model
+    self.Y     = 160  # pixel length of image for model
+    self.SX  = self.SX_R   # current visual region with shift
+    self.SY  = self.SY_R   # current visual region with shift
+    self.VPX = self.VPX_R  # current vanishing point with shift
+    self.VPY = self.VPY_R  # current vanishing point with shift
+  def shift(self, shift):
+    def to_int(fl):
+      return int(round(fl))
+    # shift comes from calibration and says how much to shift the viual region 
+    self.SX  = self.SX_R + to_int(shift[0])   # current visual region with shift
+    self.SY  = self.SY_R + to_int(shift[1])   # current visual region with shift
+    self.VPX = self.VPX_R + to_int(shift[0])  # current vanishing point with shift
+    self.VPY = self.VPY_R + to_int(shift[1])  # current vanishing point with shift
+
+class UIParams:
+  lidar_x, lidar_y, lidar_zoom = 384, 960, 8
+  lidar_car_x, lidar_car_y = lidar_x/2., lidar_y/1.1
+  car_hwidth = 1.7272/2 * lidar_zoom
+  car_front = 2.6924 * lidar_zoom
+  car_back  = 1.8796 * lidar_zoom
+  car_color = 110
+
+class VehicleParams:
+  def __init__(self, civic):
+    if civic:
+      self.wheelbase = 2.67
+      self.steer_ratio = 15.3
+      self.slip_factor = 0.0014
+    else:
+      self.wheelbase = 2.67      # from http://www.edmunds.com/acura/ilx/2016/sedan/features-specs/
+      self.steer_ratio = 15.3    # from http://www.edmunds.com/acura/ilx/2016/road-test-specs/
+      self.slip_factor = 0.0014

selfdrive/controls/controlsd.py

@@ -0,0 +1,358 @@
+#!/usr/bin/env python
+import zmq
+import numpy as np
+
+from common.services import service_list
+from common.realtime import sec_since_boot, set_realtime_priority, Ratekeeper
+
+from selfdrive.config import CruiseButtons
+from selfdrive.config import Conversions as CV
+
+from selfdrive.controls.lib.drive_helpers import learn_angle_offset
+from selfdrive.controls.lib.alert_database import process_alert, AI
+
+import selfdrive.messaging as messaging
+
+from selfdrive.controls.lib.carstate import CarState
+from selfdrive.controls.lib.carcontroller import CarController
+from selfdrive.controls.lib.longcontrol import LongControl
+from selfdrive.controls.lib.latcontrol import LatControl
+
+from selfdrive.controls.lib.pathplanner import PathPlanner
+from selfdrive.controls.lib.adaptivecruise import AdaptiveCruise
+
+def controlsd_thread(gctx, rate=100):  #rate in Hz
+  # *** log ***
+  context = zmq.Context()
+  live100 = messaging.pub_sock(context, service_list['live100'].port)
+  thermal = messaging.sub_sock(context, service_list['thermal'].port)
+  live20 = messaging.sub_sock(context, service_list['live20'].port)
+  model = messaging.sub_sock(context, service_list['model'].port)
+
+  logcan = messaging.sub_sock(context, service_list['can'].port)
+  sendcan = messaging.pub_sock(context, service_list['sendcan'].port)
+
+  # *** init the major players ***
+  CS = CarState(logcan)
+  CC = CarController()
+
+  PP = PathPlanner(model)
+  AC = AdaptiveCruise(live20)
+
+  LoC = LongControl()
+  LaC = LatControl()
+
+  # *** control initial values ***
+  apply_brake = 0
+  enabled = False
+
+  # *** time values ***
+  last_enable_pressed = 0
+
+  # *** controls initial values ***
+  # *** display stuff
+  soft_disable_start = 0
+  sounding = False
+  no_mismatch_pcm_last, no_mismatch_ctrl_last = 0, 0
+
+  # car state
+  alert, sound_exp, hud_exp, text_exp, alert_p = None, 0, 0, 0, 0
+  rear_view_cam, rear_view_toggle = False, False
+
+  v_cruise = 255           # this means no display
+  v_cruise_max = 144
+  v_cruise_min = 8
+  v_cruise_delta = 8
+
+  # on activation target at least 25mph. With 5mph you need too much tapping
+  v_cruise_enable_min = 40
+
+  hud_v_cruise = 255
+
+  angle_offset = 0
+
+  max_enable_speed = 57.   # ~91 mph
+
+  pcm_threshold = 25.*CV.MPH_TO_MS    # below this speed pcm cancels
+
+  overtemp = True
+
+  # 0.0 - 1.0
+  awareness_status = 0.0
+
+  # start the loop
+  set_realtime_priority(2)
+
+  rk = Ratekeeper(rate)
+  while 1:
+    cur_time = sec_since_boot()
+
+    # read CAN
+    canMonoTimes = CS.update(logcan)
+
+    # **** rearview mirror management ***
+    if CS.cruise_setting == 1 and CS.prev_cruise_setting == 0:
+      rear_view_toggle = not rear_view_toggle
+
+    # show rear view camera on phone if in reverse gear or when lkas button is pressed
+    rear_view_cam = (CS.gear_shifter == 2) or rear_view_toggle or CS.blinker_on
+
+    # *** thermal checking logic ***
+
+    # thermal data, checked every second
+    td = messaging.recv_sock(thermal)
+    if td is not None:
+      cpu_temps = [td.thermal.cpu0, td.thermal.cpu1, td.thermal.cpu2,
+                   td.thermal.cpu3, td.thermal.mem, td.thermal.gpu]
+      # check overtemp
+      overtemp = any(t > 950 for t in cpu_temps)
+
+    # *** getting model logic ***
+    PP.update(cur_time, CS.v_ego)
+
+    if rk.frame % 5 == 2:
+      # *** run this at 20hz again ***
+      angle_offset = learn_angle_offset(enabled, CS.v_ego, angle_offset, np.asarray(PP.d_poly), LaC.y_des, CS.steer_override)
+
+    # to avoid race conditions, check if control has been disabled for at least 0.2s
+    mismatch_ctrl = not CC.controls_allowed and enabled
+    mismatch_pcm = (not CS.pcm_acc_status and (not apply_brake or CS.v_ego < 0.1)) and enabled
+
+    # keep resetting start timer if mismatch isn't true
+    if not mismatch_ctrl:
+      no_mismatch_ctrl_last = cur_time
+    if not mismatch_pcm or not CS.brake_only:
+      no_mismatch_pcm_last = cur_time
+
+    #*** v_cruise logic ***
+    if CS.brake_only:
+      v_cruise = int(CS.v_cruise_pcm) # TODO: why sometimes v_cruise_pcm is long type?
+    else:
+      if CS.prev_cruise_buttons == 0 and CS.cruise_buttons == CruiseButtons.RES_ACCEL and enabled:
+        v_cruise = v_cruise - (v_cruise % v_cruise_delta) + v_cruise_delta
+      elif CS.prev_cruise_buttons == 0 and CS.cruise_buttons == CruiseButtons.DECEL_SET and enabled:
+        v_cruise = v_cruise + (v_cruise % v_cruise_delta) - v_cruise_delta
+
+    # *** enabling/disabling logic ***
+    enable_pressed = (CS.prev_cruise_buttons == CruiseButtons.DECEL_SET or CS.prev_cruise_buttons == CruiseButtons.RES_ACCEL) \
+                     and CS.cruise_buttons == 0
+
+    if enable_pressed:
+      print "enabled pressed at", cur_time
+      last_enable_pressed = cur_time
+
+    # if pcm does speed control than we need to wait on pcm to enable
+    if CS.brake_only:
+      enable_condition = (cur_time - last_enable_pressed) < 0.2 and CS.pcm_acc_status
+    else:
+      enable_condition = enable_pressed
+
+    # always clear the alert at every cycle
+    alert_id = []
+
+    # check for PCM not enabling
+    if CS.brake_only and (cur_time - last_enable_pressed) < 0.2 and not CS.pcm_acc_status:
+      print "waiting for PCM to enable"
+
+    # check for denied enabling
+    if enable_pressed and not enabled:
+      deny_enable = \
+        [(AI.SEATBELT, not CS.seatbelt),
+         (AI.DOOR_OPEN, not CS.door_all_closed),
+         (AI.ESP_OFF, CS.esp_disabled),
+         (AI.STEER_ERROR, CS.steer_error),
+         (AI.BRAKE_ERROR, CS.brake_error),
+         (AI.GEAR_NOT_D, not CS.gear_shifter_valid),
+         (AI.MAIN_OFF, not CS.main_on),
+         (AI.PEDAL_PRESSED, CS.user_gas_pressed or CS.brake_pressed or (CS.pedal_gas > 0 and CS.brake_only)),
+         (AI.HIGH_SPEED, CS.v_ego > max_enable_speed),
+         (AI.OVERHEAT, overtemp),
+         (AI.COMM_ISSUE, PP.dead or AC.dead),
+         (AI.CONTROLSD_LAG, rk.remaining < -0.2)]
+      for alertn, cond in deny_enable:
+        if cond:
+          alert_id += [alertn]
+
+    # check for soft disables
+    if enabled:
+      soft_disable = \
+        [(AI.SEATBELT_SD, not CS.seatbelt),
+         (AI.DOOR_OPEN_SD, not CS.door_all_closed),
+         (AI.ESP_OFF_SD, CS.esp_disabled),
+         (AI.OVERHEAT_SD, overtemp),
+         (AI.COMM_ISSUE_SD, PP.dead or AC.dead),
+         (AI.CONTROLSD_LAG_SD, rk.remaining < -0.2)]
+      sounding = False
+      for alertn, cond in soft_disable:
+        if cond:
+          alert_id += [alertn]
+          sounding = True
+          # soft disengagement expired, user need to take control
+          if (cur_time - soft_disable_start) > 3.:
+            enabled = False
+            v_cruise = 255
+      if not sounding:
+        soft_disable_start = cur_time
+
+    # check for immediate disables
+    if enabled:
+      immediate_disable = \
+        [(AI.PCM_LOW_SPEED, (cur_time > no_mismatch_pcm_last > 0.2) and CS.v_ego < pcm_threshold),
+         (AI.STEER_ERROR_ID, CS.steer_error),
+         (AI.BRAKE_ERROR_ID, CS.brake_error),
+         (AI.CTRL_MISMATCH_ID, (cur_time - no_mismatch_ctrl_last) > 0.2),
+         (AI.PCM_MISMATCH_ID, (cur_time - no_mismatch_pcm_last) > 0.2)]
+      for alertn, cond in immediate_disable:
+        if cond:
+          alert_id += [alertn]
+          # immediate turn off control
+          enabled = False
+          v_cruise = 255
+
+    # user disabling
+    if enabled and (CS.user_gas_pressed or CS.brake_pressed or not CS.gear_shifter_valid or \
+         (CS.cruise_buttons == CruiseButtons.CANCEL and CS.prev_cruise_buttons == 0) or \
+         not CS.main_on or (CS.pedal_gas > 0 and CS.brake_only)):
+      enabled = False
+      v_cruise = 255
+      alert_id += [AI.DISABLE]
+
+    # enabling
+    if enable_condition and not enabled and len(alert_id) == 0:
+      print "*** enabling controls"
+
+      #enable both lateral and longitudinal controls
+      enabled = True
+      counter_pcm_enabled = CS.counter_pcm
+      # on activation, let's always set v_cruise from where we are, even if PCM ACC is active
+      # what we want to be displayed in mph
+      v_cruise_mph = round(CS.v_ego * CV.MS_TO_MPH * CS.ui_speed_fudge)
+      # what we need to send to have that displayed
+      v_cruise = int(round(np.maximum(v_cruise_mph * CV.MPH_TO_KPH, v_cruise_enable_min)))
+
+      # 6 minutes driver you're on
+      awareness_status = 1.0
+
+      # reset the PID loops
+      LaC.reset()
+      # start long control at actual speed
+      LoC.reset(v_pid = CS.v_ego)
+
+      alert_id += [AI.ENABLE]
+
+    if v_cruise != 255 and not CS.brake_only:
+      v_cruise = np.clip(v_cruise, v_cruise_min, v_cruise_max)
+
+    # **** awareness status manager ****
+    if enabled:
+      # gives the user 6 minutes
+      awareness_status -= 1.0/(100*60*6)
+      # reset on steering, blinker, or cruise buttons
+      if CS.steer_override or CS.blinker_on or CS.cruise_buttons or CS.cruise_setting:
+        awareness_status = 1.0
+      if awareness_status <= 0.:
+        alert_id += [AI.DRIVER_DISTRACTED]
+
+    # ****** initial actuators commands ***
+    # *** gas/brake PID loop ***
+    AC.update(cur_time, CS.v_ego, CS.angle_steers, LoC.v_pid, awareness_status, CS.VP)
+    final_gas, final_brake = LoC.update(enabled, CS, v_cruise, AC.v_target_lead, AC.a_target, AC.jerk_factor)
+    pcm_accel = int(np.clip(AC.a_pcm/1.4,0,1)*0xc6)   # TODO: perc of max accel in ACC?
+
+    # *** steering PID loop ***
+    final_steer, sat_flag = LaC.update(enabled, CS, PP.d_poly, angle_offset)
+
+    # this needs to stay before hysteresis logic to avoid pcm staying on control during brake hysteresis
+    pcm_override = True   # this is always True
+    pcm_cancel_cmd = False
+    if CS.brake_only and final_brake == 0.:
+      pcm_speed = LoC.v_pid - .3  # FIXME: just for exp
+    else:
+      pcm_speed = 0
+
+    # ***** handle alerts ****
+    # send a "steering required alert" if saturation count has reached the limit
+    if sat_flag:
+      alert_id += [AI.STEER_SATURATED]
+
+    # process the alert, based on id
+    alert, chime, beep, hud_alert, alert_text, sound_exp, hud_exp, text_exp, alert_p = \
+      process_alert(alert_id, alert, cur_time, sound_exp, hud_exp, text_exp, alert_p)
+
+    # alerts pub
+    if len(alert_id) != 0:
+      print alert_id, alert_text
+
+    # *** process for hud display ***
+    if not enabled or (hud_v_cruise == 255 and CS.counter_pcm == counter_pcm_enabled):
+      hud_v_cruise = 255
+    else:
+      hud_v_cruise = v_cruise
+
+    # *** actually do can sends ***
+    CC.update(sendcan, enabled, CS, rk.frame, \
+      final_gas, final_brake, final_steer, \
+      pcm_speed, pcm_override, pcm_cancel_cmd, pcm_accel, \
+      hud_v_cruise, hud_show_lanes = enabled, \
+      hud_show_car = AC.has_lead, \
+      hud_alert = hud_alert, \
+      snd_beep = beep, snd_chime = chime)
+
+    # ***** publish state to logger *****
+
+    # publish controls state at 100Hz
+    dat = messaging.new_message()
+    dat.init('live100')
+
+    # move liveUI into live100
+    dat.live100.rearViewCam = bool(rear_view_cam)
+    dat.live100.alertText1 = alert_text[0]
+    dat.live100.alertText2 = alert_text[1]
+    dat.live100.awarenessStatus = max(awareness_status, 0.0) if enabled else 0.0
+
+    # what packets were used to process
+    dat.live100.canMonoTimes = canMonoTimes
+    dat.live100.mdMonoTime = PP.logMonoTime
+    dat.live100.l20MonoTime = AC.logMonoTime
+
+    # if controls is enabled
+    dat.live100.enabled = enabled
+
+    # car state
+    dat.live100.vEgo = float(CS.v_ego)
+    dat.live100.aEgo = float(CS.a_ego)
+    dat.live100.angleSteers = float(CS.angle_steers)
+    dat.live100.hudLead = CS.hud_lead
+    dat.live100.steerOverride = CS.steer_override
+
+    # longitudinal control state
+    dat.live100.vPid = float(LoC.v_pid)
+    dat.live100.vCruise = float(v_cruise)
+    dat.live100.upAccelCmd = float(LoC.Up_accel_cmd)
+    dat.live100.uiAccelCmd = float(LoC.Ui_accel_cmd)
+
+    # lateral control state
+    dat.live100.yActual = float(LaC.y_actual)
+    dat.live100.yDes = float(LaC.y_des)
+    dat.live100.upSteer = float(LaC.Up_steer)
+    dat.live100.uiSteer = float(LaC.Ui_steer)
+
+    # processed radar state, should add a_pcm?
+    dat.live100.vTargetLead = float(AC.v_target_lead)
+    dat.live100.aTargetMin = float(AC.a_target[0])
+    dat.live100.aTargetMax = float(AC.a_target[1])
+    dat.live100.jerkFactor = float(AC.jerk_factor)
+
+    # lag
+    dat.live100.cumLagMs = -rk.remaining*1000.
+
+    live100.send(dat.to_bytes())
+
+    # *** run loop at fixed rate ***
+    rk.keep_time()
+
+def main(gctx=None):
+  controlsd_thread(gctx, 100)
+
+if __name__ == "__main__":
+  main()

selfdrive/controls/lib/adaptivecruise.py

@@ -0,0 +1,332 @@
+import selfdrive.messaging as messaging
+import numpy as np
+
+# lookup tables VS speed to determine min and max accels in cruise
+_A_CRUISE_MIN_V  = np.asarray([-1.0, -.8, -.67, -.5, -.30])
+_A_CRUISE_MIN_BP = np.asarray([   0., 5.,  10., 20.,  40.])
+
+# need fast accel at very low speed for stop and go
+_A_CRUISE_MAX_V  = np.asarray([1., 1., .8, .5, .30])
+_A_CRUISE_MAX_BP = np.asarray([0.,  5., 10., 20., 40.])
+
+def calc_cruise_accel_limits(v_ego):
+  a_cruise_min = np.interp(v_ego, _A_CRUISE_MIN_BP, _A_CRUISE_MIN_V)
+  a_cruise_max = np.interp(v_ego, _A_CRUISE_MAX_BP, _A_CRUISE_MAX_V)
+
+  a_pcm = 1.  # always 1 for now
+  return np.vstack([a_cruise_min, a_cruise_max]), a_pcm
+
+_A_TOTAL_MAX_V = np.asarray([1.5, 1.9, 3.2])
+_A_TOTAL_MAX_BP = np.asarray([0., 20., 40.])
+
+def limit_accel_in_turns(v_ego, angle_steers, a_target, a_pcm, VP):
+  #*** this function returns a limited long acceleration allowed, depending on the existing lateral acceleration
+  # this should avoid accelerating when losing the target in turns
+  deg_to_rad = np.pi / 180.  # from can reading to rad
+
+  a_total_max = np.interp(v_ego, _A_TOTAL_MAX_BP, _A_TOTAL_MAX_V)
+  a_y = v_ego**2 * angle_steers * deg_to_rad / (VP.steer_ratio * VP.wheelbase)
+  a_x_allowed = np.sqrt(np.maximum(a_total_max**2 - a_y**2, 0.))
+
+  a_target[1] = np.minimum(a_target[1], a_x_allowed)
+  a_pcm = np.minimum(a_pcm, a_x_allowed)
+  return a_target, a_pcm
+
+def process_a_lead(a_lead):
+  # soft threshold of 0.5m/s^2 applied to a_lead to reject noise, also not considered positive a_lead
+  a_lead_threshold = 0.5
+  a_lead = np.minimum(a_lead + a_lead_threshold, 0)
+  return a_lead
+
+def calc_desired_distance(v_lead):
+  #*** compute desired distance ***
+  t_gap = 1.7  # good to be far away
+  d_offset = 4 # distance when at zero speed
+  return d_offset + v_lead * t_gap
+
+
+#linear slope
+_L_SLOPE_V = np.asarray([0.40, 0.10])
+_L_SLOPE_BP = np.asarray([0.,  40])
+
+# parabola slope
+_P_SLOPE_V = np.asarray([1.0, 0.25])
+_P_SLOPE_BP = np.asarray([0., 40])
+
+def calc_desired_speed(d_lead, d_des, v_lead, a_lead):
+  #*** compute desired speed ***
+  # the desired speed curve is divided in 4 portions: 
+  # 1-constant
+  # 2-linear to regain distance
+  # 3-linear to shorten distance
+  # 4-parabolic (constant decel)
+
+  max_runaway_speed = -2. # no slower than 2m/s over the lead
+
+  # interpolate the lookups to find the slopes for a give lead speed
+  l_slope = np.interp(v_lead, _L_SLOPE_BP, _L_SLOPE_V)
+  p_slope = np.interp(v_lead, _P_SLOPE_BP, _P_SLOPE_V)
+
+  # this is where parabola and linear curves are tangents  
+  x_linear_to_parabola = p_slope / l_slope**2
+
+  # parabola offset to have the parabola being tangent to the linear curve
+  x_parabola_offset = p_slope / (2 * l_slope**2)
+
+  if d_lead < d_des:
+    # calculate v_rel_des on the line that connects 0m at max_runaway_speed to d_des
+    v_rel_des_1 = (- max_runaway_speed) / d_des * (d_lead - d_des)
+    # calculate v_rel_des on one third of the linear slope
+    v_rel_des_2 = (d_lead - d_des) * l_slope / 3.
+    # take the min of the 2 above
+    v_rel_des = np.minimum(v_rel_des_1, v_rel_des_2)
+    v_rel_des = np.maximum(v_rel_des, max_runaway_speed)
+  elif d_lead < d_des + x_linear_to_parabola:
+    v_rel_des = (d_lead - d_des) * l_slope
+    v_rel_des = np.maximum(v_rel_des, max_runaway_speed)
+  else:
+    v_rel_des = np.sqrt(2 * (d_lead - d_des - x_parabola_offset) * p_slope)
+
+  # compute desired speed
+  v_target = v_rel_des + v_lead
+
+  # compute v_coast: above this speed we want to coast
+  t_lookahead = 1.   # how far in time we consider a_lead to anticipate the coast region
+  v_coast_shift = np.maximum(a_lead * t_lookahead, - v_lead)   # don't consider projections that would make v_lead<0
+  v_coast = (v_lead + v_target)/2 + v_coast_shift              # no accel allowed above this line
+  v_coast = np.minimum(v_coast, v_target)
+
+  return v_target, v_coast
+
+def calc_critical_decel(d_lead, v_rel, d_offset, v_offset):
+  # this function computes the required decel to avoid crashing, given safety offsets
+  a_critical =  - np.maximum(0., v_rel + v_offset)**2/np.maximum(2*(d_lead - d_offset), 0.5)
+  return a_critical
+
+
+# maximum acceleration adjustment
+_A_CORR_BY_SPEED_V = np.asarray([0.4, 0.4, 0])
+# speeds
+_A_CORR_BY_SPEED_BP = np.asarray([0., 5., 20.])
+
+def calc_positive_accel_limit(d_lead, d_des, v_ego, v_rel, v_ref, v_rel_ref, v_coast, v_target, a_lead_contr, a_max):
+  a_coast_min = -1.0   # never coast faster then -1m/s^2
+  # coasting behavior above v_coast. Forcing a_max to be negative will force the pid_speed to decrease,
+  # regardless v_target
+  if v_ref > np.minimum(v_coast, v_target):
+    # for smooth coast we can be agrressive and target a point where car would actually crash
+    v_offset_coast = 0.
+    d_offset_coast = d_des/2. - 4.
+
+    # acceleration value to smoothly coast until we hit v_target
+    if d_lead > d_offset_coast + 0.1:
+      a_coast = calc_critical_decel(d_lead, v_rel_ref, d_offset_coast, v_offset_coast)
+      # if lead is decelerating, then offset the coast decel
+      a_coast += a_lead_contr
+      a_max = np.maximum(a_coast, a_coast_min)
+    else:
+      a_max = a_coast_min
+  else:
+    # same as cruise accel, but add a small correction based on lead acceleration at low speeds
+    # when lead car accelerates faster, we can do the same, and vice versa
+
+    a_max = a_max + np.interp(v_ego, _A_CORR_BY_SPEED_BP, _A_CORR_BY_SPEED_V) \
+                  * np.clip(-v_rel / 4., -.5, 1)
+  return a_max
+
+# arbitrary limits to avoid too high accel being computed
+_A_SAT = np.asarray([-10., 5.])
+
+# do not consider a_lead at 0m/s, fully consider it at 10m/s
+_A_LEAD_LOW_SPEED_V = np.asarray([0., 1.])
+
+# speed break points
+_A_LEAD_LOW_SPEED_BP = np.asarray([0., 10.])
+
+# add a small offset to the desired decel, just for safety margin
+_DECEL_OFFSET_V = np.asarray([-0.3, -0.5, -0.5, -0.4, -0.3])
+
+# speed bp: different offset based on the likelyhood that lead decels abruptly
+_DECEL_OFFSET_BP = np.asarray([0., 4., 15., 30, 40.])
+
+
+def calc_acc_accel_limits(d_lead, d_des, v_ego, v_pid, v_lead, v_rel, a_lead,
+                          v_target, v_coast, a_target, a_pcm):
+  #*** compute max accel ***
+  # v_rel is now your velocity in lead car frame
+  v_rel = -v_rel  # this simplifiess things when thinking in d_rel-v_rel diagram
+
+  v_rel_pid = v_pid - v_lead
+
+  # this is how much lead accel we consider in assigning the desired decel
+  a_lead_contr = a_lead * np.interp(v_lead, _A_LEAD_LOW_SPEED_BP,
+                                    _A_LEAD_LOW_SPEED_V) * 0.8
+
+  # first call of calc_positive_accel_limit is used to shape v_pid
+  a_target[1] = calc_positive_accel_limit(d_lead, d_des, v_ego, v_rel, v_pid,
+                                          v_rel_pid, v_coast, v_target,
+                                          a_lead_contr, a_target[1])
+  # second call of calc_positive_accel_limit is used to limit the pcm throttle
+  # control (only useful when we don't control throttle directly)
+  a_pcm = calc_positive_accel_limit(d_lead, d_des, v_ego, v_rel, v_ego, v_rel,
+                                    v_coast, v_target, a_lead_contr, a_pcm)
+
+  #*** compute max decel ***
+  v_offset = 1.  # assume the car is 1m/s slower
+  d_offset = 1.  # assume the distance is 1m lower
+  if v_target - v_ego > 0.5:
+    pass  # acc target speed is above vehicle speed, so we can use the cruise limits
+  elif d_lead > d_offset + 0.01:  # add small value to avoid by zero divisions
+    # compute needed accel to get to 1m distance with -1m/s rel speed
+    decel_offset = np.interp(v_lead, _DECEL_OFFSET_BP, _DECEL_OFFSET_V)
+
+    critical_decel = calc_critical_decel(d_lead, v_rel, d_offset, v_offset)
+    a_target[0] = np.minimum(decel_offset + critical_decel + a_lead_contr,
+                             a_target[0])
+  else:
+    a_target[0] = _A_SAT[0]
+  # a_min can't be higher than a_max
+  a_target[0] = np.minimum(a_target[0], a_target[1])
+  # final check on limits
+  a_target = np.clip(a_target, _A_SAT[0], _A_SAT[1])
+  a_target = a_target.tolist()
+  return a_target, a_pcm
+
+def calc_jerk_factor(d_lead, v_rel):
+  # we don't have an explicit jerk limit, so this function calculates a factor
+  # that is used by the PID controller to scale the gains. Not the cleanest solution 
+  # but we need this for the demo.
+  # TODO: Calculate Kp and Ki directly in this function.
+
+  # the higher is the decel required to avoid a crash, the higher is the PI factor scaling
+  d_offset = 0.5
+  v_offset = 2.
+  a_offset = 1.
+  jerk_factor_max = 1.0 # can't increase Kp and Ki more than double.
+  if d_lead < d_offset + 0.1: # add small value to avoid by zero divisions
+    jerk_factor = jerk_factor_max
+  else:
+    a_critical = - calc_critical_decel(d_lead, -v_rel, d_offset, v_offset)
+    # increase Kp and Ki by 20% for every 1m/s2 of decel required above 1m/s2
+    jerk_factor = np.maximum(a_critical - a_offset, 0.)/5.
+    jerk_factor = np.minimum(jerk_factor, jerk_factor_max)
+  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 = np.minimum(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 np.sqrt(delta) + v_rel < 0.1:
+    ttc = 5.
+  else:
+    ttc = 2 * d_rel / (np.sqrt(delta) + v_rel)
+  return ttc
+
+
+def limit_accel_driver_awareness(v_ego, a_target, a_pcm, awareness_status):
+  decel_bp = [0.  ,  40.]
+  decel_v  = [-0.3, -0.2]
+  decel = np.interp(v_ego, decel_bp, decel_v)
+  # gives 18 seconds before decel begins (w 6 minute timeout)
+  if awareness_status < -0.05:
+    a_target[1] = np.minimum(a_target[1], decel)
+    a_target[0] = np.minimum(a_target[1], a_target[0])
+    a_pcm = 0.
+  return a_target, a_pcm
+
+MAX_SPEED_POSSIBLE = 55.
+
+def compute_speed_with_leads(v_ego, angle_steers, v_pid, l1, l2, awareness_status, VP):
+  # drive limits
+  # TODO: Make lims function of speed (more aggressive at low speed).
+  a_lim = [-3., 1.5]
+
+  #*** set target speed pretty high, as lead hasn't been considered yet
+  v_target_lead = MAX_SPEED_POSSIBLE
+
+  #*** set accel limits as cruise accel/decel limits ***
+  a_target, a_pcm = calc_cruise_accel_limits(v_ego)
+  #*** limit max accel in sharp turns
+  a_target, a_pcm = limit_accel_in_turns(v_ego, angle_steers, a_target, a_pcm, VP)
+  jerk_factor = 0.
+
+  if l1 is not None and l1.status:
+    #*** process noisy a_lead signal from radar processing ***
+    a_lead_p = process_a_lead(l1.aLeadK)
+
+    #*** compute desired distance ***
+    d_des = calc_desired_distance(l1.vLead)
+
+    #*** compute desired speed ***
+    v_target_lead, v_coast = calc_desired_speed(l1.dRel, d_des, l1.vLead, a_lead_p)
+
+    if l2 is not None and l2.status:
+      #*** process noisy a_lead signal from radar processing ***
+      a_lead_p2 = process_a_lead(l2.aLeadK)
+
+      #*** compute desired distance ***
+      d_des2 = calc_desired_distance(l2.vLead)
+
+      #*** compute desired speed ***
+      v_target_lead2, v_coast2 = calc_desired_speed(l2.dRel, d_des2, l2.vLead, a_lead_p2)
+
+      # listen to lead that makes you go slower
+      if v_target_lead2 < v_target_lead:
+        l1 = l2
+        d_des, a_lead_p, v_target_lead, v_coast = d_des2, a_lead_p2, v_target_lead2, v_coast2
+
+    # l1 is the main lead now
+
+    #*** compute accel limits ***
+    a_target1, a_pcm1 = calc_acc_accel_limits(l1.dRel, d_des, v_ego, v_pid, l1.vLead,
+                                     l1.vRel, a_lead_p, v_target_lead, v_coast, a_target, a_pcm)
+
+    # we can now limit a_target to a_lim
+    a_target = np.clip(a_target1, a_lim[0], a_lim[1])
+    a_pcm = np.clip(a_pcm1, a_lim[0], a_lim[1]).tolist()
+
+    #*** compute max factor ***
+    jerk_factor = calc_jerk_factor(l1.dRel, l1.vRel)
+
+  # force coasting decel if driver hasn't been controlling car in a while
+  a_target, a_pcm = limit_accel_driver_awareness(v_ego, a_target, a_pcm, awareness_status)
+
+  return v_target_lead, a_target, a_pcm, jerk_factor
+
+
+class AdaptiveCruise(object):
+  def __init__(self, live20):
+    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, awareness_status, VP):
+    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
+      self.dead = False
+    elif cur_time - self.last_cal > 0.5:
+      self.dead = True
+
+    self.v_target_lead, self.a_target, self.a_pcm, self.jerk_factor = \
+      compute_speed_with_leads(v_ego, angle_steers, v_pid, self.l1, self.l2, awareness_status, VP)
+    self.has_lead = self.v_target_lead != MAX_SPEED_POSSIBLE

selfdrive/controls/lib/alert_database.py

@@ -0,0 +1,178 @@
+alerts = []
+keys = ["id",
+        "chime",
+        "beep",
+        "hud_alert",
+        "screen_chime",
+        "priority",
+        "text_line_1",
+        "text_line_2",
+        "duration_sound",
+        "duration_hud_alert",
+        "duration_text"]
+
+
+#car chimes: enumeration from dbc file. Chimes are for alerts and warnings
+class CM:
+  MUTE = 0
+  SINGLE = 3
+  DOUBLE = 4
+  REPEATED = 1
+  CONTINUOUS = 2
+
+
+#car beepss: enumeration from dbc file. Beeps are for activ and deactiv
+class BP:
+  MUTE = 0
+  SINGLE = 3
+  TRIPLE = 2
+  REPEATED = 1
+
+
+# lert ids
+class AI:
+  ENABLE = 0
+  DISABLE = 1
+  SEATBELT = 2
+  DOOR_OPEN = 3
+  PEDAL_PRESSED = 4
+  COMM_ISSUE = 5
+  ESP_OFF = 6
+  FCW = 7
+  STEER_ERROR = 8
+  BRAKE_ERROR = 9
+  CALIB_INCOMPLETE = 10
+  CALIB_INVALID = 11
+  GEAR_NOT_D = 12
+  MAIN_OFF = 13
+  STEER_SATURATED = 14
+  PCM_LOW_SPEED = 15
+  THERMAL_DEAD = 16
+  OVERHEAT = 17
+  HIGH_SPEED = 18
+  CONTROLSD_LAG = 19
+  STEER_ERROR_ID = 100
+  BRAKE_ERROR_ID = 101
+  PCM_MISMATCH_ID = 102
+  CTRL_MISMATCH_ID = 103
+  SEATBELT_SD = 200
+  DOOR_OPEN_SD = 201
+  COMM_ISSUE_SD = 202
+  ESP_OFF_SD = 203
+  THERMAL_DEAD_SD = 204
+  OVERHEAT_SD = 205
+  CONTROLSD_LAG_SD = 206
+  CALIB_INCOMPLETE_SD = 207
+  CALIB_INVALID_SD = 208
+  DRIVER_DISTRACTED = 300
+
+class AH:
+  #[alert_idx, value]
+  # See dbc files for info on values"
+  NONE           = [0, 0]
+  FCW            = [1, 0x8]
+  STEER          = [2, 1]
+  BRAKE_PRESSED  = [3, 10]
+  GEAR_NOT_D     = [4, 6]
+  SEATBELT       = [5, 5]
+  SPEED_TOO_HIGH = [6, 8]
+
+class ET:
+  ENABLE = 0
+  NO_ENTRY = 1
+  WARNING = 2
+  SOFT_DISABLE = 3
+  IMMEDIATE_DISABLE = 4
+  USER_DISABLE = 5
+
+def process_alert(alert_id, alert, cur_time, sound_exp, hud_exp, text_exp, alert_p):
+  # INPUTS:
+  # alert_id is mapped to the alert properties in alert_database
+  # cur_time is current time
+  # sound_exp is when the alert beep/chime is supposed to end 
+  # hud_exp is when the hud visual is supposed to end 
+  # text_exp is when the alert text is supposed to disappear 
+  # alert_p is the priority of the current alert 
+  # CM, BP, AH are classes defined in alert_database and they respresents chimes, beeps and hud_alerts
+  if len(alert_id) > 0:
+    # take the alert with higher priority
+    alerts_present = filter(lambda a_id: a_id['id'] in alert_id, alerts)
+    alert = sorted(alerts_present, key=lambda k: k['priority'])[-1]
+    # check if we have a more important alert
+    if alert['priority'] > alert_p:
+      alert_p = alert['priority']
+      sound_exp = cur_time + alert['duration_sound']
+      hud_exp = cur_time + alert['duration_hud_alert']
+      text_exp = cur_time + alert['duration_text']
+
+  chime = CM.MUTE
+  beep = BP.MUTE
+  if cur_time < sound_exp:
+    chime = alert['chime']
+    beep = alert['beep']
+
+  hud_alert = AH.NONE
+  if cur_time < hud_exp:
+    hud_alert = alert['hud_alert']
+
+  alert_text = ["", ""]
+  if cur_time < text_exp:
+    alert_text = [alert['text_line_1'], alert['text_line_2']]
+
+  if chime == CM.MUTE and beep == BP.MUTE and hud_alert == AH.NONE:   #and alert_text[0] is None and alert_text[1] is None:
+    alert_p = 0
+  return alert, chime, beep, hud_alert, alert_text, sound_exp, hud_exp, text_exp, alert_p
+
+def process_hud_alert(hud_alert):
+  # initialize to no alert
+  fcw_display = 0
+  steer_required = 0
+  acc_alert = 0
+  if hud_alert == AH.NONE:          # no alert 
+    pass
+  elif hud_alert == AH.FCW:         # FCW
+    fcw_display = hud_alert[1]
+  elif hud_alert == AH.STEER:       # STEER
+    steer_required = hud_alert[1]
+  else:                             # any other ACC alert
+    acc_alert = hud_alert[1]
+
+  return fcw_display, steer_required, acc_alert
+
+def app_alert(alert_add):
+  alerts.append(dict(zip(keys, alert_add)))
+
+app_alert([AI.ENABLE,              CM.MUTE,     BP.SINGLE, AH.NONE,           ET.ENABLE,            2, "",                         "",                        .2, 0., 0.])
+app_alert([AI.DISABLE,             CM.MUTE,     BP.SINGLE, AH.NONE,           ET.USER_DISABLE,      2, "",                         "",                        .2, 0., 0.])
+app_alert([AI.SEATBELT,            CM.DOUBLE,   BP.MUTE,   AH.SEATBELT,       ET.NO_ENTRY,          1, "Comma Unavailable",        "Seatbelt Unlatched",      .4, 2., 3.])
+app_alert([AI.DOOR_OPEN,           CM.DOUBLE,   BP.MUTE,   AH.NONE,           ET.NO_ENTRY,          1, "Comma Unavailable",        "Door Open",               .4, 0., 3.])
+app_alert([AI.PEDAL_PRESSED,       CM.DOUBLE,   BP.MUTE,   AH.BRAKE_PRESSED,  ET.NO_ENTRY,          1, "Comma Unavailable",        "Pedal Pressed",           .4, 2., 3.])
+app_alert([AI.COMM_ISSUE,          CM.DOUBLE,   BP.MUTE,   AH.NONE,           ET.NO_ENTRY,          1, "Comma Unavailable",        "Communcation Issues",     .4, 0., 3.])
+app_alert([AI.ESP_OFF,             CM.DOUBLE,   BP.MUTE,   AH.NONE,           ET.NO_ENTRY,          1, "Comma Unavailable",        "ESP Off",                 .4, 0., 3.])
+app_alert([AI.FCW,                 CM.REPEATED, BP.MUTE,   AH.FCW,            ET.WARNING,           3, "Risk of Collision",        "",                        1., 2., 3.])
+app_alert([AI.STEER_ERROR,         CM.DOUBLE,   BP.MUTE,   AH.NONE,           ET.NO_ENTRY,          1, "Comma Unavailable",        "Steer Error",             .4, 0., 3.])
+app_alert([AI.BRAKE_ERROR,         CM.DOUBLE,   BP.MUTE,   AH.NONE,           ET.NO_ENTRY,          1, "Comma Unavailable",        "Brake Error",             .4, 0., 3.])
+app_alert([AI.CALIB_INCOMPLETE,    CM.DOUBLE,   BP.MUTE,   AH.NONE,           ET.NO_ENTRY,          1, "Comma Unavailable",        "Calibration in Progress", .4, 0., 3.])
+app_alert([AI.CALIB_INVALID,       CM.DOUBLE,   BP.MUTE,   AH.NONE,           ET.NO_ENTRY,          1, "Comma Unavailable",        "Calibration Error",       .4, 0., 3.])
+app_alert([AI.GEAR_NOT_D,          CM.DOUBLE,   BP.MUTE,   AH.GEAR_NOT_D,     ET.NO_ENTRY,          1, "Comma Unavailable",        "Gear not in D",           .4, 2., 3.])
+app_alert([AI.MAIN_OFF,            CM.MUTE,     BP.MUTE,   AH.NONE,           ET.NO_ENTRY,          1, "Comma Unavailable",        "Main Switch Off",         .4, 0., 3.])
+app_alert([AI.STEER_SATURATED,     CM.SINGLE,   BP.MUTE,   AH.STEER,          ET.WARNING,           2, "Take Control",             "Steer Control Saturated", 1., 2., 3.])
+app_alert([AI.PCM_LOW_SPEED,       CM.MUTE,     BP.SINGLE, AH.STEER,          ET.WARNING,           2, "Comma disengaged",         "Speed too low",           .2, 2., 3.])
+app_alert([AI.THERMAL_DEAD,        CM.DOUBLE,   BP.MUTE,   AH.NONE,           ET.NO_ENTRY,          1, "Comma Unavailable",        "Thermal Unavailable",     .4, 0., 3.])
+app_alert([AI.OVERHEAT,            CM.DOUBLE,   BP.MUTE,   AH.NONE,           ET.NO_ENTRY,          1, "Comma Unavailable",        "System Overheated",       .4, 0., 3.])
+app_alert([AI.HIGH_SPEED,          CM.DOUBLE,   BP.MUTE,   AH.SPEED_TOO_HIGH, ET.NO_ENTRY,          1, "Comma Unavailable",        "Speed Too High",          .4, 2., 3.])
+app_alert([AI.CONTROLSD_LAG,       CM.DOUBLE,   BP.MUTE,   AH.NONE,           ET.NO_ENTRY,          1, "Comma Unavailable",        "Controls Lagging",        .4, 0., 3.])
+app_alert([AI.STEER_ERROR_ID,      CM.REPEATED, BP.MUTE,   AH.STEER,          ET.IMMEDIATE_DISABLE, 3, "Take Control Immediately", "Steer Error",             1., 3., 3.])
+app_alert([AI.BRAKE_ERROR_ID,      CM.REPEATED, BP.MUTE,   AH.STEER,          ET.IMMEDIATE_DISABLE, 3, "Take Control Immediately", "Brake Error",             1., 3., 3.])
+app_alert([AI.PCM_MISMATCH_ID,     CM.REPEATED, BP.MUTE,   AH.STEER,          ET.IMMEDIATE_DISABLE, 3, "Take Control Immediately", "Pcm Mismatch",            1., 3., 3.])
+app_alert([AI.CTRL_MISMATCH_ID,    CM.REPEATED, BP.MUTE,   AH.STEER,          ET.IMMEDIATE_DISABLE, 3, "Take Control Immediately", "Ctrl Mismatch",           1., 3., 3.])
+app_alert([AI.SEATBELT_SD,         CM.REPEATED, BP.MUTE,   AH.STEER,          ET.SOFT_DISABLE,      3, "Take Control Immediately", "Seatbelt Unlatched",      1., 3., 3.])
+app_alert([AI.DOOR_OPEN_SD,        CM.REPEATED, BP.MUTE,   AH.STEER,          ET.SOFT_DISABLE,      3, "Take Control Immediately", "Door Open",               1., 3., 3.])
+app_alert([AI.COMM_ISSUE_SD,       CM.REPEATED, BP.MUTE,   AH.STEER,          ET.SOFT_DISABLE,      3, "Take Control Immediately", "Technical Issues",        1., 3., 3.])
+app_alert([AI.ESP_OFF_SD,          CM.REPEATED, BP.MUTE,   AH.STEER,          ET.SOFT_DISABLE,      3, "Take Control Immediately", "ESP Off",                 1., 3., 3.])
+app_alert([AI.THERMAL_DEAD_SD,     CM.REPEATED, BP.MUTE,   AH.STEER,          ET.SOFT_DISABLE,      3, "Take Control Immediately", "Thermal Unavailable",     1., 3., 3.])
+app_alert([AI.OVERHEAT_SD,         CM.REPEATED, BP.MUTE,   AH.STEER,          ET.SOFT_DISABLE,      3, "Take Control Immediately", "System Overheated",       1., 3., 3.])
+app_alert([AI.CONTROLSD_LAG_SD,    CM.REPEATED, BP.MUTE,   AH.STEER,          ET.SOFT_DISABLE,      3, "Take Control Immediately", "Controls Lagging",        1., 3., 3.])
+app_alert([AI.CALIB_INCOMPLETE_SD, CM.REPEATED, BP.MUTE,   AH.STEER,          ET.SOFT_DISABLE,      3, "Take Control Immediately", "Calibration in Progress", 1., 3., 3.])
+app_alert([AI.CALIB_INVALID_SD,    CM.REPEATED, BP.MUTE,   AH.STEER,          ET.SOFT_DISABLE,      3, "Take Control Immediately", "Calibration Error",       1., 3., 3.])
+app_alert([AI.DRIVER_DISTRACTED,   CM.REPEATED, BP.MUTE,   AH.STEER,          ET.SOFT_DISABLE,      2, "Take Control to Regain Speed", "User Distracted",     1., 1., 1.])

selfdrive/controls/lib/can_parser.py

@@ -0,0 +1,123 @@
+import os
+import dbcs
+from collections import defaultdict
+
+from selfdrive.controls.lib.hondacan import fix
+from common.realtime import sec_since_boot
+from common.dbc import dbc
+
+class CANParser(object):
+  def __init__(self, dbc_f, signals, checks=[]):
+    ### input:
+    # dbc_f   : dbc file
+    # signals : List of tuples (name, address, ival) where
+    #             - name is the signal name.
+    #             - address is the corresponding message address.
+    #             - ival is the initial value.
+    # checks  : List of pairs (address, frequency) where
+    #             - address is the message address of a message for which health should be
+    #               monitored.
+    #             - frequency is the frequency at which health should be monitored.
+
+    self.msgs_ck = [check[0] for check in checks]
+    self.frqs = [check[1] for check in checks]
+    self.can_valid = False  # start with False CAN assumption
+    self.msgs_upd = []      # list of updated messages
+    # list of received msg we want to monitor counter and checksum for
+    # read dbc file
+    self.can_dbc = dbc(os.path.join(dbcs.DBC_PATH, dbc_f))
+    # initialize variables to initial values
+    self.vl = {}    # signal values
+    self.ts = {}    # time stamp recorded in log
+    self.ct = {}    # current time stamp
+    self.ok = {}    # valid message?
+    self.cn = {}    # message counter
+    self.cn_vl = {} # message counter mismatch value
+    self.ck = {}    # message checksum status
+
+    for _, addr, _ in signals:
+      self.vl[addr] = {}
+      self.ts[addr] = 0
+      self.ct[addr] = sec_since_boot()
+      self.ok[addr] = False
+      self.cn[addr] = 0
+      self.cn_vl[addr] = 0
+      self.ck[addr] = False
+
+    for name, addr, ival in signals:
+      self.vl[addr][name] = ival
+
+    self._msgs = [s[1] for s in signals]
+    self._sgs = [s[0] for s in signals]
+
+    self._message_indices = defaultdict(list)
+    for i, x in enumerate(self._msgs):
+      self._message_indices[x].append(i)
+
+  def update_can(self, can_recv):
+    self.msgs_upd = []
+    cn_vl_max = 5   # no more than 5 wrong counter checks
+
+    # we are subscribing to PID_XXX, else data from USB
+    for msg, ts, cdat in can_recv:
+      idxs = self._message_indices[msg]
+      if idxs:
+        self.msgs_upd += [msg]
+        # read the entire message
+        out = self.can_dbc.decode([msg, 0, cdat])[1]
+        # checksum check
+        self.ck[msg] = True
+        if "CHECKSUM" in out.keys() and msg in self.msgs_ck:
+          # remove checksum (half byte)
+          ck_portion = (''.join((cdat[:-1], '0'))).decode('hex')
+          # recalculate checksum
+          msg_vl = fix(ck_portion, msg)
+          # compare recalculated vs received checksum
+          if msg_vl != cdat.decode('hex'):
+            print hex(msg), "CHECKSUM FAIL"
+            self.ck[msg] = False
+            self.ok[msg] = False
+        # counter check
+        cn = 0
+        if "COUNTER" in out.keys():
+          cn = out["COUNTER"]
+        # check counter validity if it's a relevant message
+        if cn != ((self.cn[msg] + 1) % 4) and msg in self.msgs_ck and "COUNTER" in out.keys():
+          #print hex(msg), "FAILED COUNTER!"
+          self.cn_vl[msg] += 1   # counter check failed
+        else:
+          self.cn_vl[msg] -= 1   # counter check passed
+        # message status is invalid if we received too many wrong counter values
+        if self.cn_vl[msg] >= cn_vl_max:
+          self.ok[msg] = False
+
+        # update msg time stamps and counter value
+        self.ts[msg] = ts
+        self.ct[msg] = sec_since_boot()
+        self.cn[msg] = cn
+        self.cn_vl[msg] = min(max(self.cn_vl[msg], 0), cn_vl_max)
+
+        # set msg valid status if checksum is good and wrong counter counter is zero
+        if self.ck[msg] and self.cn_vl[msg] == 0:
+          self.ok[msg] = True
+
+        # update value of signals in the
+        for ii in idxs:
+          sg = self._sgs[ii]
+          self.vl[msg][sg] = out[sg]
+
+  # for each message, check if it's too long since last time we received it
+    self._check_dead_msgs()
+
+    # assess overall can validity: if there is one relevant message invalid, then set can validity flag to False
+    self.can_valid = True
+    if False in self.ok.values():
+      print "CAN INVALID!"
+      self.can_valid = False
+
+  def _check_dead_msgs(self):
+    ### input:
+    ## simple stuff for now: msg is not valid if a message isn't received for 10 consecutive steps
+    for msg in set(self._msgs):
+      if msg in self.msgs_ck and sec_since_boot() - self.ct[msg] > 10./self.frqs[self.msgs_ck.index(msg)]:
+        self.ok[msg] = False

selfdrive/controls/lib/carcontroller.py

@@ -0,0 +1,185 @@
+from collections import namedtuple
+
+import common.numpy_fast as np
+import selfdrive.controls.lib.hondacan as hondacan
+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.alert_database import process_hud_alert
+from selfdrive.controls.lib.drive_helpers import actuator_hystereses, rate_limit
+
+HUDData = namedtuple("HUDData",
+                     ["pcm_accel", "v_cruise", "X2", "car", "X4", "X5",
+                      "lanes", "beep", "X8", "chime", "acc_alert"])
+
+class CarController(object):
+  def __init__(self):
+    self.controls_allowed = False
+    self.mismatch_start, self.pcm_mismatch_start = 0, 0
+    self.braking = False
+    self.brake_steady = 0.
+    self.final_brake_last = 0.
+
+  def update(self, sendcan, enabled, CS, frame, final_gas, final_brake, final_steer, \
+             pcm_speed, pcm_override, pcm_cancel_cmd, pcm_accel, \
+             hud_v_cruise, hud_show_lanes, hud_show_car, hud_alert, \
+             snd_beep, snd_chime):
+    """ Controls thread """
+
+    # *** apply brake hysteresis ***
+    final_brake, self.braking, self.brake_steady = actuator_hystereses(final_brake, self.braking, self.brake_steady, CS.v_ego, CS.civic)
+
+    # *** no output if not enabled ***
+    if not enabled:
+      final_gas = 0.
+      final_brake = 0.
+      final_steer = 0.
+      # send pcm acc cancel cmd if drive is disabled but pcm is still on, or if the system can't be activated
+      if CS.pcm_acc_status:
+        pcm_cancel_cmd = True
+
+    # *** rate limit after the enable check ***
+    final_brake = rate_limit(final_brake, self.final_brake_last, -2., 1./100)
+    self.final_brake_last = final_brake
+
+    # vehicle hud display, wait for one update from 10Hz 0x304 msg
+    #TODO: use enum!!
+    if hud_show_lanes:
+      hud_lanes = 0x04
+    else:
+      hud_lanes = 0x00
+
+    # TODO: factor this out better
+    if enabled:
+      if hud_show_car:
+        hud_car = 0xe0
+      else:
+        hud_car = 0xd0
+    else:
+      hud_car = 0xc0
+
+    #print chime, alert_id, hud_alert
+    fcw_display, steer_required, acc_alert = process_hud_alert(hud_alert)
+
+    hud = HUDData(pcm_accel, hud_v_cruise, 0x41, hud_car,
+                  0xc1, 0x41, hud_lanes + steer_required,
+                  snd_beep, 0x48, (snd_chime << 5) + fcw_display, acc_alert)
+
+    if not all(isinstance(x, int) and 0 <= x < 256 for x in hud):
+      print "INVALID HUD", hud
+      hud = HUDData(0xc6, 255, 64, 0xc0, 209, 0x41, 0x40, 0, 0x48, 0, 0)
+
+    # **** process the car messages ****
+
+    user_brake_ctrl = CS.user_brake/0.015625  # FIXME: factor needed to convert to old scale
+
+    # *** compute control surfaces ***
+    tt = sec_since_boot()
+    GAS_MAX = 1004
+    BRAKE_MAX = 1024/4
+    #STEER_MAX = 0xF00 if not CS.torque_mod else 0xF00/4    # ilx has 8x steering torque limit, used as a 2x
+    STEER_MAX = 0xF00  # ilx has 8x steering torque limit, used as a 2x
+    GAS_OFFSET = 328
+
+    # steer torque is converted back to CAN reference (positive when steering right)
+    apply_gas = int(np.clip(final_gas*GAS_MAX, 0, GAS_MAX-1))
+    apply_brake = int(np.clip(final_brake*BRAKE_MAX, 0, BRAKE_MAX-1))
+    apply_steer = int(np.clip(-final_steer*STEER_MAX, -STEER_MAX, STEER_MAX))
+
+    # no gas if you are hitting the brake or the user is
+    if apply_gas > 0 and (apply_brake != 0 or user_brake_ctrl > 10):
+      print "CANCELLING GAS", apply_brake, user_brake_ctrl
+      apply_gas = 0
+
+    # no computer brake if the user is hitting the gas
+    # if the computer is trying to brake, it can't be hitting the gas
+    # TODO: car_gas can override brakes without canceling... this is bad 
+    if CS.car_gas > 0 and apply_brake != 0:
+      apply_brake = 0
+
+    if (CS.prev_cruise_buttons == CruiseButtons.DECEL_SET or CS.prev_cruise_buttons == CruiseButtons.RES_ACCEL) and \
+        CS.cruise_buttons == 0 and not self.controls_allowed:
+      print "CONTROLS ARE LIVE"
+      self.controls_allowed = True
+
+    # to avoid race conditions, check if control has been disabled for at least 0.2s
+    # keep resetting start timer if mismatch isn't true
+    if not (self.controls_allowed and not enabled):
+      self.mismatch_start = tt
+
+    # to avoid race conditions, check if control is disabled but pcm control is on for at least 0.2s
+    if not (not self.controls_allowed and CS.pcm_acc_status):
+      self.pcm_mismatch_start = tt
+
+    # something is very wrong, since pcm control is active but controls should not be allowed; TODO: send pcm fault cmd?
+    if (tt - self.pcm_mismatch_start) > 0.2:
+      pcm_cancel_cmd = True
+
+    # TODO: clean up gear condition, ideally only D (and P for debug) shall be valid gears
+    if (CS.cruise_buttons == CruiseButtons.CANCEL or CS.brake_pressed or
+        CS.user_gas_pressed or (tt - self.mismatch_start) > 0.2 or
+        not CS.main_on or not CS.gear_shifter_valid or
+        (CS.pedal_gas > 0 and CS.brake_only)) and self.controls_allowed:
+      self.controls_allowed = False
+
+    # 5 is a permanent fault, no torque request will be fullfilled
+    if CS.steer_error:
+      print "STEER ERROR"
+      self.controls_allowed = False
+
+    # any other cp.vl[0x18F]['STEER_STATUS'] is common and can happen during user override. sending 0 torque to avoid EPS sending error 5
+    elif CS.steer_not_allowed:
+      print "STEER ALERT, TORQUE INHIBITED"
+      apply_steer = 0
+
+    if CS.brake_error:
+      print "BRAKE ERROR"
+      self.controls_allowed = False
+
+    if not CS.can_valid and self.controls_allowed:   # 200 ms
+      print "CAN INVALID"
+      self.controls_allowed = False
+
+    if not self.controls_allowed:
+      apply_steer = 0
+      apply_gas = 0
+      apply_brake = 0
+      pcm_speed = 0          # make sure you send 0 target speed to pcm
+      #pcm_cancel_cmd = 1    # prevent pcm control from turning on. FIXME: we can't just do this
+
+    # Send CAN commands.
+    can_sends = []
+
+    # Send steering command.
+    idx = frame % 4
+    can_sends.append(hondacan.create_steering_control(apply_steer, idx))
+
+    # Send gas and brake commands.
+    if (frame % 2) == 0:
+      idx = (frame / 2) % 4
+      can_sends.append(
+        hondacan.create_brake_command(apply_brake, pcm_override,
+                                      pcm_cancel_cmd, hud.chime, idx))
+
+      if not CS.brake_only:
+        # send exactly zero if apply_gas is zero. Interceptor will send the max between read value and apply_gas. 
+        # This prevents unexpected pedal range rescaling
+        gas_amount = (apply_gas + GAS_OFFSET) * (apply_gas > 0)
+        can_sends.append(hondacan.create_gas_command(gas_amount, idx))
+
+    # Send dashboard UI commands.
+    if (frame % 10) == 0:
+      idx = (frame/10) % 4
+      can_sends.extend(hondacan.create_ui_commands(pcm_speed, hud, CS.civic, idx))
+
+    # radar at 20Hz, but these msgs need to be sent at 50Hz on ilx (seems like an Acura bug)
+    if CS.civic:
+      radar_send_step = 5
+    else:
+      radar_send_step = 2
+
+    if (frame % radar_send_step) == 0:
+      idx = (frame/radar_send_step) % 4
+      can_sends.extend(hondacan.create_radar_commands(CS.v_ego, CS.civic, idx))
+
+    sendcan.send(can_list_to_can_capnp(can_sends).to_bytes())

selfdrive/controls/lib/carstate.py

@@ -0,0 +1,275 @@
+import numpy as np
+
+import selfdrive.messaging as messaging
+from selfdrive.boardd.boardd import can_capnp_to_can_list_old, can_capnp_to_can_list
+from selfdrive.controls.lib.can_parser import CANParser
+from selfdrive.controls.lib.fingerprints import fingerprints
+from selfdrive.config import VehicleParams
+from common.realtime import sec_since_boot
+
+
+def get_can_parser(civic, brake_only):
+  # this function generates lists for signal, messages and initial values
+  if civic:
+    dbc_f = 'honda_civic_touring_2016_can.dbc'
+    signals = [
+      ("XMISSION_SPEED", 0x158, 0),
+      ("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),
+    ]
+    checks = [
+      (0x14a, 100),
+      (0x158, 100),
+      (0x17c, 100),
+      (0x191, 100),
+      (0x1a4, 50),
+      (0x326, 10),
+      (0x1b0, 50),
+      (0x1d0, 50),
+      (0x305, 10),
+      (0x324, 10),
+      (0x405, 3),
+    ]
+
+  else:
+    dbc_f = 'acura_ilx_2016_can.dbc'
+    signals = [
+      ("XMISSION_SPEED", 0x158, 0),
+      ("WHEEL_SPEED_FL", 0x1d0, 0),
+      ("WHEEL_SPEED_FR", 0x1d0, 0),
+      ("WHEEL_SPEED_RL", 0x1d0, 0),
+      ("STEER_ANGLE", 0x156, 0),
+      ("STEER_TORQUE_SENSOR", 0x18f, 0),
+      ("GEAR", 0x1a3, 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", 0x1a6, 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", 0x1a3, 0),
+      ("MAIN_ON", 0x1a6, 0),
+      ("ACC_STATUS", 0x17c, 0),
+      ("PEDAL_GAS", 0x17c, 0),
+      ("CRUISE_SETTING", 0x1a6, 0),
+      ("LEFT_BLINKER", 0x294, 0),
+      ("RIGHT_BLINKER", 0x294, 0),
+      ("COUNTER", 0x324, 0),
+    ]
+    checks = [
+      (0x156, 100),
+      (0x158, 100),
+      (0x17c, 100),
+      (0x1a3, 50),
+      (0x1a4, 50),
+      (0x1a6, 50),
+      (0x1b0, 50),
+      (0x1d0, 50),
+      (0x305, 10),
+      (0x324, 10),
+      (0x405, 3),
+    ]
+
+  # add gas interceptor reading if we are using it
+  if not brake_only:
+    signals.append(("INTERCEPTOR_GAS", 0x201, 0))
+    checks.append((0x201, 50))
+
+  return CANParser(dbc_f, signals, checks)
+
+def fingerprint(logcan):
+  print "waiting for fingerprint..."
+  brake_only = True
+
+  finger = {}
+  st = None
+  while 1:
+    possible_cars = []
+    for a in messaging.drain_sock(logcan, wait_for_one=True):
+      if st is None:
+        st = sec_since_boot()
+      for adr, _, msg, idx in can_capnp_to_can_list(a):
+        # pedal
+        if adr == 0x201 and idx == 0:
+          brake_only = False
+        if idx == 0:
+          finger[adr] = len(msg)
+
+    # check for a single match
+    for f in fingerprints:
+      is_possible = True
+      for adr in finger:
+        # confirm all messages we have seen match
+        if adr not in fingerprints[f] or fingerprints[f][adr] != finger[adr]:
+          #print "mismatch", f, adr
+          is_possible = False
+          break
+      if is_possible:
+        possible_cars.append(f)
+
+    # if we only have one car choice and it's been 100ms since we got our first message, exit
+    if len(possible_cars) == 1 and st is not None and (sec_since_boot()-st) > 0.1:
+      break
+    elif len(possible_cars) == 0:
+      raise Exception("car doesn't match any fingerprints")
+
+  print "fingerprinted", possible_cars[0]
+  return brake_only, possible_cars[0]
+
+class CarState(object):
+  def __init__(self, logcan):
+    self.torque_mod = False
+    self.brake_only, self.car_type = fingerprint(logcan)
+
+    # assuming if you have a pedal interceptor you also have a torque mod
+    if not self.brake_only:
+      self.torque_mod = True
+
+    if self.car_type == "HONDA CIVIC 2016 TOURING":
+      self.civic = True
+    elif self.car_type == "ACURA ILX 2016 ACURAWATCH PLUS":
+      self.civic = False
+    else:
+      raise ValueError("unsupported car %s" % self.car_type)
+
+    # initialize can parser
+    self.cp = get_can_parser(self.civic, self.brake_only)
+
+    self.user_gas, self.user_gas_pressed = 0., 0
+
+    self.cruise_buttons = 0
+    self.cruise_setting = 0
+    self.blinker_on = 0
+
+    # TODO: actually make this work
+    self.a_ego = 0.
+
+    # speed in UI is shown as few % higher
+    self.ui_speed_fudge = 1.01 if self.civic else 1.025
+
+    # load vehicle params
+    self.VP = VehicleParams(self.civic)
+
+  def update(self, logcan):
+    # ******************* do can recv *******************
+    can_pub_main = []
+    canMonoTimes = []
+    for a in messaging.drain_sock(logcan):
+      canMonoTimes.append(a.logMonoTime)
+      can_pub_main.extend(can_capnp_to_can_list_old(a, [0,2]))
+
+    cp = self.cp
+    cp.update_can(can_pub_main)
+
+    # copy can_valid
+    self.can_valid = cp.can_valid
+
+    # car params
+    v_weight_v  = [0., 1. ]  # don't trust smooth speed at low values to avoid premature zero snapping
+    v_weight_bp = [1., 6.]   # smooth blending, below ~0.6m/s the smooth speed snaps to zero
+
+    # update prevs, update must run once per loop
+    self.prev_cruise_buttons = self.cruise_buttons
+    self.prev_cruise_setting = self.cruise_setting
+    self.prev_blinker_on = self.blinker_on
+
+    # ******************* parse out can *******************
+    self.door_all_closed = not any([cp.vl[0x405]['DOOR_OPEN_FL'], cp.vl[0x405]['DOOR_OPEN_FR'],
+                               cp.vl[0x405]['DOOR_OPEN_RL'], cp.vl[0x405]['DOOR_OPEN_RR']])
+    self.seatbelt = not cp.vl[0x305]['SEATBELT_DRIVER_LAMP'] and cp.vl[0x305]['SEATBELT_DRIVER_LATCHED']
+    # error 2 = temporary
+    # error 4 = temporary, hit a bump
+    # error 5 (permanent)
+    # error 6 = temporary
+    # error 7 (permanent)
+    #self.steer_error = cp.vl[0x18F]['STEER_STATUS'] in [5,7]
+    # whitelist instead of blacklist, safer at the expense of disengages
+    self.steer_error = cp.vl[0x18F]['STEER_STATUS'] not in [0,2,4,6]
+    self.steer_not_allowed = cp.vl[0x18F]['STEER_STATUS'] != 0
+    if cp.vl[0x18F]['STEER_STATUS'] != 0:
+      print cp.vl[0x18F]['STEER_STATUS']
+    self.brake_error = cp.vl[0x1B0]['BRAKE_ERROR_1'] or cp.vl[0x1B0]['BRAKE_ERROR_2']
+    self.esp_disabled = cp.vl[0x1A4]['ESP_DISABLED']
+    # calc best v_ego estimate, by averaging two opposite corners
+    self.v_wheel = (
+      cp.vl[0x1D0]['WHEEL_SPEED_FL'] + cp.vl[0x1D0]['WHEEL_SPEED_FR'] +
+      cp.vl[0x1D0]['WHEEL_SPEED_RL'] + cp.vl[0x1D0]['WHEEL_SPEED_RR']) / 4.
+    # blend in transmission speed at low speed, since it has more low speed accuracy
+    self.v_weight = np.interp(self.v_wheel, v_weight_bp, v_weight_v)
+    self.v_ego = (1. - self.v_weight) * cp.vl[0x158]['XMISSION_SPEED'] + self.v_weight * self.v_wheel
+    if not self.brake_only:
+      self.user_gas = cp.vl[0x201]['INTERCEPTOR_GAS']
+      self.user_gas_pressed = self.user_gas > 0 # this works because interceptor read < 0 when pedal position is 0. Once calibrated, this will change
+      #print user_gas, user_gas_pressed
+    if self.civic:
+      self.gear_shifter = cp.vl[0x191]['GEAR_SHIFTER']
+      self.angle_steers = cp.vl[0x14A]['STEER_ANGLE']
+      self.gear = 0  # TODO: civic has CVT... needs rev engineering
+      self.cruise_setting = cp.vl[0x296]['CRUISE_SETTING']
+      self.cruise_buttons = cp.vl[0x296]['CRUISE_BUTTONS']
+      self.main_on = cp.vl[0x326]['MAIN_ON']
+      self.gear_shifter_valid = self.gear_shifter in [1,8]  # TODO: 1/P allowed for debug
+      self.blinker_on = cp.vl[0x326]['LEFT_BLINKER'] or cp.vl[0x326]['RIGHT_BLINKER']
+    else:
+      self.gear_shifter = cp.vl[0x1A3]['GEAR_SHIFTER']
+      self.angle_steers = cp.vl[0x156]['STEER_ANGLE']
+      self.gear = cp.vl[0x1A3]['GEAR']
+      self.cruise_setting = cp.vl[0x1A6]['CRUISE_SETTING']
+      self.cruise_buttons = cp.vl[0x1A6]['CRUISE_BUTTONS']
+      self.main_on = cp.vl[0x1A6]['MAIN_ON']
+      self.gear_shifter_valid = self.gear_shifter in [1,4]  # TODO: 1/P allowed for debug
+      self.blinker_on = cp.vl[0x294]['LEFT_BLINKER'] or cp.vl[0x294]['RIGHT_BLINKER']
+    self.car_gas = cp.vl[0x130]['CAR_GAS']
+    self.brake_pressed = cp.vl[0x17C]['BRAKE_PRESSED']
+    self.user_brake = cp.vl[0x1A4]['USER_BRAKE']
+    self.standstill = not cp.vl[0x1B0]['WHEELS_MOVING']
+    self.steer_override = abs(cp.vl[0x18F]['STEER_TORQUE_SENSOR']) > 1200
+    self.v_cruise_pcm = cp.vl[0x324]['CRUISE_SPEED_PCM']
+    self.pcm_acc_status = cp.vl[0x17C]['ACC_STATUS']
+    self.pedal_gas = cp.vl[0x17C]['PEDAL_GAS']
+    self.hud_lead = cp.vl[0x30C]['HUD_LEAD']
+    self.counter_pcm = cp.vl[0x324]['COUNTER']
+
+    return canMonoTimes

selfdrive/controls/lib/drive_helpers.py

@@ -0,0 +1,52 @@
+import numpy as np
+
+def rate_limit(new_value, last_value, dw_step, up_step):
+  return np.clip(new_value, last_value + dw_step, last_value + up_step)
+
+def learn_angle_offset(lateral_control, v_ego, angle_offset, d_poly, y_des, steer_override):
+  # simple integral controller that learns how much steering offset to put to have the car going straight
+  min_offset = -1.  # deg
+  max_offset =  1.  # deg
+  alpha = 1./36000. # correct by 1 deg in 2 mins, at 30m/s, with 50cm of error, at 20Hz
+  min_learn_speed = 1.
+
+  # learn less at low speed or when turning
+  alpha_v = alpha*(np.maximum(v_ego - min_learn_speed, 0.))/(1. + 0.5*abs(y_des))
+
+  # only learn if lateral control is active and if driver is not overriding:
+  if lateral_control and not steer_override:
+    angle_offset += d_poly[3] * alpha_v
+    angle_offset = np.clip(angle_offset, min_offset, max_offset)
+
+  return angle_offset
+
+def actuator_hystereses(final_brake, braking, brake_steady, v_ego, civic):
+  # hyst params... TODO: move these to VehicleParams
+  brake_hyst_on = 0.055 if civic else 0.1    # to activate brakes exceed this value
+  brake_hyst_off = 0.005                     # to deactivate brakes below this value
+  brake_hyst_gap = 0.01                      # don't change brake command for small ocilalitons within this value
+
+  #*** histeresys logic to avoid brake blinking. go above 0.1 to trigger
+  if (final_brake < brake_hyst_on and not braking) or final_brake < brake_hyst_off:
+    final_brake = 0.
+  braking = final_brake > 0.
+
+  # for small brake oscillations within brake_hyst_gap, don't change the brake command
+  if final_brake == 0.:
+    brake_steady = 0.
+  elif final_brake > brake_steady + brake_hyst_gap:
+    brake_steady = final_brake - brake_hyst_gap
+  elif final_brake < brake_steady - brake_hyst_gap:
+    brake_steady = final_brake + brake_hyst_gap
+  final_brake = brake_steady
+
+  if not civic:
+    brake_on_offset_v  = [.25, .15]   # min brake command on brake activation. below this no decel is perceived
+    brake_on_offset_bp = [15., 30.]     # offset changes VS speed to not have too abrupt decels at high speeds
+    # offset the brake command for threshold in the brake system. no brake torque perceived below it
+    brake_on_offset = np.interp(v_ego, brake_on_offset_bp, brake_on_offset_v)
+    brake_offset = brake_on_offset - brake_hyst_on
+    if final_brake > 0.0:
+      final_brake += brake_offset
+
+  return final_brake, braking, brake_steady

selfdrive/controls/lib/fingerprints.py

@@ -0,0 +1,8 @@
+fingerprints = {
+  "ACURA ILX 2016 ACURAWATCH PLUS": {
+    1024L: 5, 513L: 5, 1027L: 5, 1029L: 8, 929L: 4, 1057L: 5, 777L: 8, 1034L: 5, 1036L: 8, 398L: 3, 399L: 7, 145L: 8, 660L: 8, 985L: 3, 923L: 2, 542L: 7, 773L: 7, 800L: 8, 432L: 7, 419L: 8, 420L: 8, 1030L: 5, 422L: 8, 808L: 8, 428L: 8, 304L: 8, 819L: 7, 821L: 5, 57L: 3, 316L: 8, 545L: 4, 464L: 8, 1108L: 8, 597L: 8, 342L: 6, 983L: 8, 344L: 8, 804L: 8, 1039L: 8, 476L: 4, 892L: 8, 490L: 8, 1064L: 7, 882L: 2, 884L: 7, 887L: 8, 888L: 8, 380L: 8, 1365L: 5
+  },
+  "HONDA CIVIC 2016 TOURING": {
+    1024L: 5, 513L: 5, 1027L: 5, 1029L: 8, 777L: 8, 1036L: 8, 1039L: 8, 1424L: 5, 401L: 8, 148L: 8, 662L: 4, 985L: 3, 795L: 8, 773L: 7, 800L: 8, 545L: 6, 420L: 8, 806L: 8, 808L: 8, 1322L: 5, 427L: 3, 428L: 8, 304L: 8, 432L: 7, 57L: 3, 450L: 8, 929L: 8, 330L: 8, 1302L: 8, 464L: 8, 1361L: 5, 1108L: 8, 597L: 8, 470L: 2, 344L: 8, 804L: 8, 399L: 7, 476L: 7, 1633L: 8, 487L: 4, 892L: 8, 490L: 8, 493L: 5, 884L: 8, 891L: 8, 380L: 8, 1365L: 5
+  }
+}

selfdrive/controls/lib/hondacan.py

@@ -0,0 +1,88 @@
+import struct
+
+import common.numpy_fast as np
+from selfdrive.config import Conversions as CV
+
+
+# *** Honda specific ***
+def can_cksum(mm):
+  s = 0
+  for c in mm:
+    c = ord(c)
+    s += (c>>4)
+    s += c & 0xF
+  s = 8-s
+  s %= 0x10
+  return s
+
+def fix(msg, addr):
+  msg2 = msg[0:-1] + chr(ord(msg[-1]) | can_cksum(struct.pack("I", addr)+msg))
+  return msg2
+
+def make_can_msg(addr, dat, idx, alt):
+  if idx is not None:
+    dat += chr(idx << 4)
+    dat = fix(dat, addr)
+  return [addr, 0, dat, alt]
+
+def create_brake_command(apply_brake, pcm_override, pcm_cancel_cmd, chime, idx):
+  """Creates a CAN message for the Honda DBC BRAKE_COMMAND."""
+  pump_on = apply_brake > 0
+  brakelights = apply_brake > 0
+  brake_rq = apply_brake > 0
+
+  pcm_fault_cmd = False
+  amount = struct.pack("!H", (apply_brake << 6) + pump_on)
+  msg = amount + struct.pack("BBB", (pcm_override << 4) |
+                             (pcm_fault_cmd << 2) |
+                             (pcm_cancel_cmd << 1) | brake_rq, 0x80,
+                             brakelights << 7) + chr(chime) + "\x00"
+  return make_can_msg(0x1fa, msg, idx, 0)
+
+def create_gas_command(gas_amount, idx):
+  """Creates a CAN message for the Honda DBC GAS_COMMAND."""
+  msg = struct.pack("!H", gas_amount)
+  return make_can_msg(0x200, msg, idx, 0)
+
+def create_steering_control(apply_steer, idx):
+  """Creates a CAN message for the Honda DBC STEERING_CONTROL."""
+  msg = struct.pack("!h", apply_steer) + ("\x80\x00" if apply_steer != 0 else "\x00\x00")
+  return make_can_msg(0xe4, msg, idx, 0)
+
+def create_ui_commands(pcm_speed, hud, civic, idx):
+  """Creates an iterable of CAN messages for the UIs."""
+  commands = []
+  pcm_speed_real = np.clip(int(round(pcm_speed / 0.002763889)), 0,
+                           64000)  # conversion factor from dbc file
+  msg_0x30c = struct.pack("!HBBBBB", pcm_speed_real, hud.pcm_accel,
+                          hud.v_cruise, hud.X2, hud.car, hud.X4)
+  commands.append(make_can_msg(0x30c, msg_0x30c, idx, 0))
+
+  msg_0x33d = chr(hud.X5) + chr(hud.lanes) + chr(hud.beep) + chr(hud.X8)
+  commands.append(make_can_msg(0x33d, msg_0x33d, idx, 0))
+  if civic:  # 2 more msgs
+    msg_0x35e = chr(0) * 7
+    commands.append(make_can_msg(0x35e, msg_0x35e, idx, 0))
+    msg_0x39f = (
+      chr(0) * 2 + chr(hud.acc_alert) + chr(0) + chr(0xff) + chr(0x7f) + chr(0)
+    )
+    commands.append(make_can_msg(0x39f, msg_0x39f, idx, 0))
+  return commands
+
+def create_radar_commands(v_ego, civic, idx):
+  """Creates an iterable of CAN messages for the radar system."""
+  commands = []
+  v_ego_kph = np.clip(int(round(v_ego * CV.MS_TO_KPH)), 0, 255)
+  speed = struct.pack('!B', v_ego_kph)
+  msg_0x300 = ("\xf9" + speed + "\x8a\xd0" +\
+              ("\x20" if idx == 0 or idx == 3 else "\x00") +\
+                "\x00\x00")
+  if civic:
+    msg_0x301 = "\x02\x38\x44\x32\x4f\x00\x00"
+    # add 8 on idx.
+    commands.append(make_can_msg(0x300, msg_0x300, idx + 8, 1))
+  else:
+    msg_0x301 = "\x0f\x18\x51\x02\x5a\x00\x00"
+    commands.append(make_can_msg(0x300, msg_0x300, idx, 1))
+  commands.append(make_can_msg(0x301, msg_0x301, idx, 1))
+  return commands

selfdrive/controls/lib/latcontrol.py

@@ -0,0 +1,120 @@
+import numpy as np
+
+def calc_curvature(v_ego, angle_steers, VP, angle_offset=0):
+  deg_to_rad = np.pi/180.
+  angle_steers_rad = (angle_steers - angle_offset) * deg_to_rad
+  curvature = angle_steers_rad/(VP.steer_ratio * VP.wheelbase * (1. + VP.slip_factor * v_ego**2))
+  return curvature
+
+def calc_d_lookahead(v_ego):
+  #*** this function computes how far too look for lateral control
+  # howfar we look ahead is function of speed
+  offset_lookahead = 1.
+  coeff_lookahead = 4.4
+  # sqrt on speed is needed to keep, for a given curvature, the y_offset 
+  # proportional to speed. Indeed, y_offset is prop to d_lookahead^2
+  # 26m at 25m/s
+  d_lookahead = offset_lookahead + np.sqrt(np.maximum(v_ego, 0)) * coeff_lookahead
+  return d_lookahead
+
+def calc_lookahead_offset(v_ego, angle_steers, d_lookahead, VP, angle_offset):
+  #*** this function return teh lateral offset given the steering angle, speed and the lookahead distance
+  curvature = calc_curvature(v_ego, angle_steers, VP, angle_offset)
+
+  # clip is to avoid arcsin NaNs due to too sharp turns
+  y_actual = d_lookahead * np.tan(np.arcsin(np.clip(d_lookahead * curvature, -0.999, 0.999))/2.)
+  return y_actual, curvature
+
+def pid_lateral_control(v_ego, y_actual, y_des, Ui_steer, steer_max,
+                        steer_override, sat_count, enabled, half_pid, rate):
+
+  sat_count_rate = 1./rate
+  sat_count_limit = 0.8       # after 0.8s of continuous saturation, an alert will be sent
+
+  error_steer = y_des - y_actual
+  Ui_unwind_speed = 0.3/rate   #.3 per second
+  if not half_pid:
+    Kp, Ki = 12.0, 1.0
+  else:
+    Kp, Ki = 6.0, .5           # 2x limit in ILX
+  Up_steer = error_steer*Kp
+  Ui_steer_new = Ui_steer + error_steer*Ki * 1./rate
+  output_steer_new = Ui_steer_new + Up_steer
+
+  # Anti-wind up for integrator: do not integrate if we are against the steer limits
+  if (
+    (error_steer >= 0. and (output_steer_new < steer_max or Ui_steer < 0)) or
+    (error_steer <= 0. and
+     (output_steer_new > -steer_max or Ui_steer > 0))) and not steer_override:
+    #update integrator
+    Ui_steer = Ui_steer_new
+  # unwind integrator if driver is maneuvering the steering wheel
+  elif steer_override:
+    Ui_steer -= Ui_unwind_speed * np.sign(Ui_steer)
+
+  # still, intergral term should not be bigger then limits
+  Ui_steer = np.clip(Ui_steer, -steer_max, steer_max)
+
+  output_steer = Up_steer + Ui_steer
+
+  # don't run steer control if at very low speed
+  if v_ego < 0.3 or not enabled:
+    output_steer = 0.
+    Ui_steer = 0.
+
+  # useful to know if control is against the limit
+  lateral_control_sat = False
+  if abs(output_steer) > steer_max:
+    lateral_control_sat = True
+
+  output_steer = np.clip(output_steer, -steer_max, steer_max)
+
+  # if lateral control is saturated for a certain period of time, send an alert for taking control of the car
+  # wind
+  if lateral_control_sat and not steer_override and v_ego > 10 and abs(error_steer) > 0.1:
+    sat_count += sat_count_rate
+  # unwind
+  else:
+    sat_count -= sat_count_rate
+
+  sat_flag = False
+  if sat_count >= sat_count_limit:
+    sat_flag = True
+
+  sat_count = np.clip(sat_count, 0, 1)
+
+  return output_steer, Up_steer, Ui_steer, lateral_control_sat, sat_count, sat_flag
+
+class LatControl(object):
+  def __init__(self):
+    self.Up_steer = 0.
+    self.sat_count = 0
+    self.y_des = 0.0
+    self.lateral_control_sat = False
+    self.Ui_steer = 0.
+    self.reset()
+
+  def reset(self):
+    self.Ui_steer = 0.
+
+  def update(self, enabled, CS, d_poly, angle_offset):
+    rate = 100
+
+    steer_max = 1.0
+
+    # how far we look ahead is function of speed
+    d_lookahead = calc_d_lookahead(CS.v_ego)
+
+    # calculate actual offset at the lookahead point
+    self.y_actual, _ = calc_lookahead_offset(CS.v_ego, CS.angle_steers,
+                                             d_lookahead, CS.VP, angle_offset)
+
+    # desired lookahead offset
+    self.y_des = np.polyval(d_poly, d_lookahead)
+
+    output_steer, self.Up_steer, self.Ui_steer, self.lateral_control_sat, self.sat_count, sat_flag = pid_lateral_control(
+      CS.v_ego, self.y_actual, self.y_des, self.Ui_steer, steer_max,
+      CS.steer_override, self.sat_count, enabled, CS.torque_mod, rate)
+
+    final_steer = np.clip(output_steer, -steer_max, steer_max)
+    return final_steer, sat_flag

selfdrive/controls/lib/longcontrol.py

@@ -0,0 +1,232 @@
+import numpy as np
+from selfdrive.config import Conversions as CV
+
+class LongCtrlState:
+  #*** this function handles the long control state transitions
+  # long_control_state labels:
+  off =      0  # Off
+  pid =      1  # moving and tracking targets, with PID control running
+  stopping = 2  # stopping and changing controls to almost open loop as PID does not fit well at such a low speed
+  starting = 3  # starting and releasing brakes in open loop before giving back to PID
+
+def long_control_state_trans(enabled, long_control_state, v_ego, v_target, v_pid, output_gb):
+
+  stopping_speed = 0.5
+  stopping_target_speed = 0.3
+  starting_target_speed = 0.5
+  brake_threshold_to_pid = 0.2
+
+  stopping_condition = ((v_ego < stopping_speed) and (v_pid < stopping_target_speed) and (v_target < stopping_target_speed))
+
+  if not enabled:
+    long_control_state = LongCtrlState.off
+  else:
+    if long_control_state == LongCtrlState.off:
+      if enabled:
+        long_control_state = LongCtrlState.pid
+    elif long_control_state == LongCtrlState.pid:
+      if stopping_condition:
+        long_control_state = LongCtrlState.stopping
+    elif long_control_state == LongCtrlState.stopping:
+      if (v_target > starting_target_speed):
+        long_control_state = LongCtrlState.starting
+    elif long_control_state == LongCtrlState.starting:
+      if stopping_condition:
+        long_control_state = LongCtrlState.stopping
+      elif output_gb >= -brake_threshold_to_pid:
+        long_control_state = LongCtrlState.pid
+
+  return long_control_state
+
+def get_compute_gb():
+  # see debug/dump_accel_from_fiber.py
+  w0 = np.array([[ 1.22056961, -0.39625418,  0.67952657],
+                 [ 1.03691769,  0.78210306, -0.41343188]])
+  b0 = np.array([ 0.01536703, -0.14335321, -0.26932889])
+  w2 = np.array([[-0.59124422,  0.42899439,  0.38660881],
+                 [ 0.79973811,  0.13178682,  0.08550351],
+                 [-0.15651935, -0.44360259,  0.76910877]])
+  b2 = np.array([ 0.15624429,  0.02294923, -0.0341086 ])
+  w4 = np.array([[-0.31521443],
+                 [-0.38626176],
+                 [ 0.52667892]])
+  b4 = np.array([-0.02922216])
+
+  def compute_output(dat, w0, b0, w2, b2, w4, b4):
+    m0 = np.dot(dat, w0) + b0
+    m0 = leakyrelu(m0, 0.1)
+    m2 = np.dot(m0, w2) + b2
+    m2 = leakyrelu(m2, 0.1)
+    m4 = np.dot(m2, w4) + b4
+    return m4
+
+  def leakyrelu(x, alpha):
+    return np.maximum(x, alpha * x)
+
+  def _compute_gb(dat):
+    #linearly extrap below v1 using v1 and v2 data
+    v1 = 5.
+    v2 = 10.
+    vx = dat[1]
+    if vx > 5.:
+      m4 = compute_output(dat, w0, b0, w2, b2, w4, b4)
+    else:
+      dat[1] = v1
+      m4v1 = compute_output(dat, w0, b0, w2, b2, w4, b4)
+      dat[1] = v2
+      m4v2 = compute_output(dat, w0, b0, w2, b2, w4, b4)
+      m4 = (vx - v1) * (m4v2 - m4v1) / (v2 - v1) + m4v1
+    return m4
+  return _compute_gb
+
+# takes in [desired_accel, current_speed] -> [-1.0, 1.0] where -1.0 is max brake and 1.0 is max gas
+compute_gb = get_compute_gb()
+
+def pid_long_control(v_ego, v_pid, Ui_accel_cmd, gas_max, brake_max, jerk_factor, gear, rate):
+  #*** This function compute the gb pedal positions in order to track the desired speed
+  # proportional and integral terms. More precision at low speed
+  Kp_v =  [1.2, 0.8, 0.5]
+  Kp_bp = [0., 5., 35.]
+  Kp = np.interp(v_ego, Kp_bp, Kp_v)
+  Ki_v =  [0.18, 0.12]
+  Ki_bp = [0., 35.]
+  Ki = np.interp(v_ego, Ki_bp, Ki_v)
+
+  # scle Kp and Ki by jerk factor drom drive_thread
+  Kp = (1. + jerk_factor)*Kp
+  Ki = (1. + jerk_factor)*Ki
+
+  # this is ugly but can speed reports 0 when speed<0.3m/s and we can't have that jump  
+  v_ego_min = 0.3
+  v_ego = np.maximum(v_ego, v_ego_min)
+
+  v_error = v_pid - v_ego
+
+  Up_accel_cmd = v_error*Kp
+  Ui_accel_cmd_new = Ui_accel_cmd + v_error*Ki*1.0/rate
+  accel_cmd_new = Ui_accel_cmd_new + Up_accel_cmd
+  output_gb_new = compute_gb([accel_cmd_new, v_ego])
+
+  # Anti-wind up for integrator: only update integrator if we not against the thottle and brake limits
+  # do not wind up if we are changing gear and we are on the gas pedal
+  if (((v_error >= 0. and (output_gb_new < gas_max or Ui_accel_cmd < 0)) or
+       (v_error <= 0. and (output_gb_new > - brake_max or Ui_accel_cmd > 0))) and
+       not (v_error >= 0. and gear == 11 and output_gb_new > 0)):
+    #update integrator
+    Ui_accel_cmd = Ui_accel_cmd_new
+
+  accel_cmd = Ui_accel_cmd + Up_accel_cmd
+
+  # go from accel to pedals
+  output_gb = compute_gb([accel_cmd, v_ego])
+  output_gb = output_gb[0]
+
+  # useful to know if control is against the limit
+  long_control_sat = False
+  if output_gb > gas_max or output_gb < -brake_max:
+    long_control_sat = True
+
+  output_gb = np.clip(output_gb, -brake_max, gas_max)
+
+  return output_gb, Up_accel_cmd, Ui_accel_cmd, long_control_sat
+
+
+stopping_brake_rate = 0.2    # brake_travel/s while trying to stop
+starting_brake_rate = 0.6    # brake_travel/s while releasing on restart
+starting_Ui = 0.5            # Since we don't have much info about acceleration at this point, be conservative
+brake_stopping_target = 0.5  # apply at least this amount of brake to maintain the vehicle stationary
+
+max_speed_error_v  = [1.5, .8]  # max positive v_pid error VS actual speed; this avoids controls windup due to slow pedal resp
+max_speed_error_bp = [0., 30.]  # speed breakpoints
+
+class LongControl(object):
+  def __init__(self):
+    self.long_control_state = LongCtrlState.off # initialized to off
+    self.long_control_sat = False
+    self.Up_accel_cmd = 0.
+    self.last_output_gb = 0.
+    self.reset(0.)
+
+  def reset(self, v_pid):
+    self.Ui_accel_cmd = 0.
+    self.v_pid = v_pid
+
+  def update(self, enabled, CS, v_cruise, v_target_lead, a_target, jerk_factor):
+    # TODO: not every time
+    if CS.brake_only:
+      gas_max_v = [0, 0]                # values
+    else:
+      gas_max_v = [0.6, 0.6]            # values
+    gas_max_bp = [0., 100.]             # speeds
+    brake_max_v = [1.0, 1.0, 0.8, 0.8]  # values
+    brake_max_bp = [0., 5., 20., 100.]  # speeds     
+
+    # brake and gas limits
+    brake_max = np.interp(CS.v_ego, brake_max_bp, brake_max_v)
+    gas_max = np.interp(CS.v_ego, gas_max_bp, gas_max_v)
+
+    overshoot_allowance = 2.0    # overshoot allowed when changing accel sign
+
+    output_gb = self.last_output_gb
+    rate = 100
+
+    # limit max target speed based on cruise setting:
+    v_cruise_mph = round(v_cruise * CV.KPH_TO_MPH)   # what's displayed in mph on the IC
+    v_target = np.minimum(v_target_lead, v_cruise_mph * CV.MPH_TO_MS / CS.ui_speed_fudge)
+
+    max_speed_delta_up = a_target[1]*1.0/rate
+    max_speed_delta_down = a_target[0]*1.0/rate
+
+    # *** long control substate transitions
+    self.long_control_state = long_control_state_trans(enabled, self.long_control_state, CS.v_ego, v_target, self.v_pid, output_gb)
+
+    # *** long control behavior based on state
+    # TODO: move this to drive_helpers
+    # disabled
+    if self.long_control_state == LongCtrlState.off:
+      self.v_pid = CS.v_ego # do nothing
+      output_gb = 0.
+      self.Ui_accel_cmd = 0.
+    # tracking objects and driving
+    elif self.long_control_state == LongCtrlState.pid:
+      #reset v_pid close to v_ego if it was too far and new v_target is closer to v_ego
+      if ((self.v_pid > CS.v_ego + overshoot_allowance) and
+          (v_target < self.v_pid)):
+        self.v_pid = np.maximum(v_target, CS.v_ego + overshoot_allowance)
+      elif ((self.v_pid < CS.v_ego - overshoot_allowance) and
+          (v_target > self.v_pid)):
+        self.v_pid = np.minimum(v_target, CS.v_ego - overshoot_allowance)
+
+      # move v_pid no faster than allowed accel limits
+      if (v_target > self.v_pid + max_speed_delta_up):
+        self.v_pid += max_speed_delta_up
+      elif (v_target < self.v_pid + max_speed_delta_down):
+        self.v_pid += max_speed_delta_down
+      else:
+        self.v_pid = v_target
+
+      # to avoid too much wind up on acceleration, limit positive speed error
+      if not CS.brake_only:
+        max_speed_error = np.interp(CS.v_ego, max_speed_error_bp, max_speed_error_v)
+        self.v_pid = np.minimum(self.v_pid, CS.v_ego + max_speed_error)
+
+      output_gb, self.Up_accel_cmd, self.Ui_accel_cmd, self.long_control_sat = pid_long_control(CS.v_ego, self.v_pid, \
+                                  self.Ui_accel_cmd, gas_max, brake_max, jerk_factor, CS.gear, rate)
+    # intention is to stop, switch to a different brake control until we stop
+    elif self.long_control_state == LongCtrlState.stopping:
+      if CS.v_ego > 0. or output_gb > -brake_stopping_target or not CS.standstill:
+        output_gb -= stopping_brake_rate/rate
+      output_gb = np.clip(output_gb, -brake_max, gas_max)
+      self.v_pid = CS.v_ego
+      self.Ui_accel_cmd = 0.
+    # intention is to move again, release brake fast before handling control to PID
+    elif self.long_control_state == LongCtrlState.starting:
+      if output_gb < -0.2:
+        output_gb += starting_brake_rate/rate
+      self.v_pid = CS.v_ego
+      self.Ui_accel_cmd = starting_Ui
+
+    self.last_output_gb = output_gb
+    final_gas = np.clip(output_gb, 0., gas_max)
+    final_brake = -np.clip(output_gb, -brake_max, 0.)
+    return final_gas, final_brake

selfdrive/controls/lib/pathplanner.py

@@ -0,0 +1,63 @@
+import selfdrive.messaging as messaging
+import numpy as np
+X_PATH = np.arange(0.0, 50.0)
+
+def model_polyfit(points):
+  return np.polyfit(X_PATH, map(float, points), 3)
+
+# lane width http://safety.fhwa.dot.gov/geometric/pubs/mitigationstrategies/chapter3/3_lanewidth.cfm
+_LANE_WIDTH_V = np.asarray([3., 3.8])
+
+# break points of speed
+_LANE_WIDTH_BP = np.asarray([0., 31.])
+
+def calc_desired_path(l_poly, r_poly, p_poly, l_prob, r_prob, p_prob, speed):
+  #*** this function computes the poly for the center of the lane, averaging left and right polys
+  lane_width = np.interp(speed, _LANE_WIDTH_BP, _LANE_WIDTH_V)
+
+  # lanes in US are ~3.6m wide
+  half_lane_poly = np.array([0., 0., 0., lane_width / 2.])
+  if l_prob + r_prob > 0.01:
+    c_poly = ((l_poly - half_lane_poly) * l_prob +
+              (r_poly + half_lane_poly) * r_prob) / (l_prob + r_prob)
+    c_prob = np.sqrt((l_prob**2 + r_prob**2) / 2.)
+  else:
+    c_poly = np.zeros(4)
+    c_prob = 0.
+
+  p_weight = 1. # predicted path weight relatively to the center of the lane
+  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):
+  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
+
+  def update(self, cur_time, v_ego):
+    md = messaging.recv_sock(self.model)
+
+    if md is not None:
+      self.logMonoTime = md.logMonoTime
+      p_poly = model_polyfit(md.model.path.points)       # predicted path
+      p_prob = 1.                                        # model does not tell this probability yet, so set to 1 for now
+      l_poly = model_polyfit(md.model.leftLane.points)   # left line
+      l_prob = md.model.leftLane.prob                    # left line prob
+      r_poly = model_polyfit(md.model.rightLane.points)  # right line
+      r_prob = md.model.rightLane.prob                   # right line prob
+
+      self.lead_dist = md.model.lead.dist
+      self.lead_prob = md.model.lead.prob
+      self.lead_var = md.model.lead.std**2
+
+      #*** compute target path ***
+      self.d_poly, _, _ = calc_desired_path(l_poly, r_poly, p_poly, l_prob, r_prob, p_prob, v_ego)
+
+      self.last_model = cur_time
+      self.dead = False
+    elif cur_time - self.last_model > 0.5:
+      self.dead = True

selfdrive/controls/lib/radar_helpers.py

@@ -0,0 +1,256 @@
+import numpy as np
+import platform
+import os
+import sys
+
+from common.kalman.ekf import FastEKF1D, SimpleSensor
+
+# radar tracks
+SPEED, ACCEL = 0, 1   # Kalman filter states enum
+
+rate, ratev = 20., 20.    # model and radar are both at 20Hz
+ts = 1./rate
+freq_v_lat = 0.2 # Hz
+k_v_lat = 2*np.pi*freq_v_lat*ts / (1 + 2*np.pi*freq_v_lat*ts)
+
+freq_a_lead = .5 # Hz
+k_a_lead = 2*np.pi*freq_a_lead*ts / (1 + 2*np.pi*freq_a_lead*ts)
+
+# stationary qualification parameters
+v_stationary_thr = 4.   # objects moving below this speed are classified as stationary
+v_oncoming_thr   = -3.9 # needs to be a bit lower in abs value than v_stationary_thr to not leave "holes"
+v_ego_stationary = 4.   # no stationary object flag below this speed
+
+class Track(object):
+  def __init__(self):
+    self.ekf = None
+    self.stationary = True
+    self.initted = False
+
+  def update(self, d_rel, y_rel, v_rel, d_path, v_ego_t_aligned):
+    if self.initted:
+      self.dPathPrev = self.dPath
+      self.vLeadPrev = self.vLead
+      self.vRelPrev = self.vRel
+
+    # relative values, copy
+    self.dRel = d_rel   # LONG_DIST
+    self.yRel = y_rel   # -LAT_DIST
+    self.vRel = v_rel   # REL_SPEED
+
+    # compute distance to path
+    self.dPath = d_path
+
+    # computed velocity and accelerations
+    self.vLead = self.vRel + v_ego_t_aligned
+
+    if not self.initted:
+      self.aRel = 0.      # nidec gives no information about this
+      self.vLat = 0.
+      self.aLead = 0.
+    else:
+      # estimate acceleration
+      a_rel_unfilt = (self.vRel - self.vRelPrev) / ts
+      a_rel_unfilt = np.clip(a_rel_unfilt, -10., 10.)
+      self.aRel = k_a_lead * a_rel_unfilt + (1 - k_a_lead) * self.aRel
+
+      v_lat_unfilt = (self.dPath - self.dPathPrev) / ts
+      self.vLat = k_v_lat * v_lat_unfilt + (1 - k_v_lat) * self.vLat
+
+      a_lead_unfilt = (self.vLead - self.vLeadPrev) / ts
+      a_lead_unfilt = np.clip(a_lead_unfilt, -10., 10.)
+      self.aLead = k_a_lead * a_lead_unfilt + (1 - k_a_lead) * self.aLead
+
+    if self.stationary:
+      # stationary objects can become non stationary, but not the other way around
+      self.stationary = v_ego_t_aligned > v_ego_stationary and abs(self.vLead) < v_stationary_thr
+    self.oncoming = self.vLead < v_oncoming_thr
+
+    if self.ekf is None:
+      self.ekf = FastEKF1D(ts, 1e3, [0.1, 1])
+      self.ekf.state[SPEED] = self.vLead
+      self.ekf.state[ACCEL] = 0
+      self.lead_sensor = SimpleSensor(SPEED, 1, 2)
+
+      self.vLeadK = self.vLead
+      self.aLeadK = self.aLead
+    else:
+      self.ekf.update_scalar(self.lead_sensor.read(self.vLead))
+      self.ekf.predict(ts)
+      self.vLeadK = float(self.ekf.state[SPEED])
+      self.aLeadK = float(self.ekf.state[ACCEL])
+
+    if not self.initted:
+      self.cnt = 1
+      self.vision_cnt = 0
+    else:
+      self.cnt += 1
+
+    self.initted = True
+    self.vision = False
+
+  def mix_vision(self, dist_to_vision, rel_speed_diff):
+    # rel speed is very hard to estimate from vision
+    if dist_to_vision < 4.0 and rel_speed_diff < 10.:
+      # vision point is never stationary
+      self.stationary = False
+      self.vision = True
+      self.vision_cnt += 1
+
+  def get_key_for_cluster(self):
+    # Weigh y higher since radar is inaccurate in this dimension
+    return [self.dRel, self.dPath*2, self.vRel]
+
+# ******************* Cluster *******************
+
+if platform.machine() == 'aarch64':
+  for x in sys.path:
+    pp = os.path.join(x, "phonelibs/hierarchy/lib")
+    if os.path.isfile(os.path.join(pp, "_hierarchy.so")):
+      sys.path.append(pp)
+      break
+  import _hierarchy
+else:
+  from scipy.cluster import _hierarchy
+
+def fcluster(Z, t, criterion='inconsistent', depth=2, R=None, monocrit=None):
+  # supersimplified function to get fast clustering. Got it from scipy
+  Z = np.asarray(Z, order='c')
+  n = Z.shape[0] + 1
+  T = np.zeros((n,), dtype='i')
+  _hierarchy.cluster_dist(Z, T, float(t), int(n))
+  return T
+
+RDR_TO_LDR = 2.7
+
+def mean(l):
+  return sum(l)/len(l)
+
+class Cluster(object):
+  def __init__(self):
+    self.tracks = set()
+
+  def add(self, t):
+    # add the first track
+    self.tracks.add(t)
+
+  # TODO: make generic
+  @property
+  def dRel(self):
+    return mean([t.dRel for t in self.tracks])
+
+  @property
+  def yRel(self):
+    return mean([t.yRel for t in self.tracks])
+
+  @property
+  def vRel(self):
+    return mean([t.vRel for t in self.tracks])
+
+  @property
+  def aRel(self):
+    return mean([t.aRel for t in self.tracks])
+
+  @property
+  def vLead(self):
+    return mean([t.vLead for t in self.tracks])
+
+  @property
+  def aLead(self):
+    return mean([t.aLead for t in self.tracks])
+
+  @property
+  def dPath(self):
+    return mean([t.dPath for t in self.tracks])
+
+  @property
+  def vLat(self):
+    return mean([t.vLat for t in self.tracks])
+
+  @property
+  def vLeadK(self):
+    return mean([t.vLeadK for t in self.tracks])
+
+  @property
+  def aLeadK(self):
+    return mean([t.aLeadK for t in self.tracks])
+
+  @property
+  def vision(self):
+    return any([t.vision for t in self.tracks])
+
+  @property
+  def vision_cnt(self):
+    return max([t.vision_cnt for t in self.tracks])
+
+  @property
+  def stationary(self):
+    return all([t.stationary for t in self.tracks])
+
+  @property
+  def oncoming(self):
+    return all([t.oncoming for t in self.tracks])
+
+  def toLive20(self, lead):
+    lead.dRel = float(self.dRel) - RDR_TO_LDR
+    lead.yRel = float(self.yRel)
+    lead.vRel = float(self.vRel)
+    lead.aRel = float(self.aRel)
+    lead.vLead = float(self.vLead)
+    lead.aLead = float(self.aLead)
+    lead.dPath = float(self.dPath)
+    lead.vLat = float(self.vLat)
+    lead.vLeadK = float(self.vLeadK)
+    lead.aLeadK = float(self.aLeadK)
+    lead.status = True
+    lead.fcw = False
+
+  def __str__(self):
+    ret = "x: %7.2f  y: %7.2f  v: %7.2f  a: %7.2f" % (self.dRel, self.yRel, self.vRel, self.aRel)
+    if self.stationary:
+      ret += " stationary"
+    if self.vision:
+      ret += " vision"
+    if self.oncoming:
+      ret += " oncoming"
+    if self.vision_cnt > 0:
+      ret += " vision_cnt: %6.0f" % self.vision_cnt
+    return ret
+
+  def is_potential_lead(self, v_ego, enabled):
+    # predict cut-ins by extrapolating lateral speed by a lookahead time
+    # lookahead time depends on cut-in distance. more attentive for close cut-ins
+    # also, above 50 meters the predicted path isn't very reliable
+
+    # the distance at which v_lat matters is higher at higher speed
+    lookahead_dist = 40. + v_ego/1.2   #40m at 0mph, ~70m at 80mph
+
+    t_lookahead_v  = [1., 0.]
+    t_lookahead_bp = [10., lookahead_dist]
+
+    # average dist
+    d_path = self.dPath
+
+    if enabled:
+      t_lookahead = np.interp(self.dRel, t_lookahead_bp, t_lookahead_v)
+      # correct d_path for lookahead time, considering only cut-ins and no more than 1m impact
+      lat_corr = np.clip(t_lookahead * self.vLat, -1, 0)
+    else:
+      lat_corr = 0.
+    d_path = np.maximum(d_path + lat_corr, 0)
+
+    if 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
+      if (self.dRel - lead_cluster.dRel) < 8. and abs(self.vRel - lead_cluster.vRel) < 1.:
+        return False
+      else:
+        return True
+    else:
+      return False

selfdrive/controls/radard.py

@@ -0,0 +1,268 @@
+#!/usr/bin/env python
+import zmq
+import numpy as np
+import numpy.matlib
+from collections import defaultdict
+
+from fastcluster import linkage_vector
+
+import selfdrive.messaging as messaging
+from selfdrive.boardd.boardd import can_capnp_to_can_list_old
+from selfdrive.controls.lib.latcontrol import calc_lookahead_offset
+from selfdrive.controls.lib.can_parser import CANParser
+from selfdrive.controls.lib.pathplanner import PathPlanner
+from selfdrive.config import VehicleParams
+from selfdrive.controls.lib.radar_helpers import Track, Cluster, fcluster, RDR_TO_LDR
+
+from common.services import service_list
+from common.realtime import sec_since_boot, set_realtime_priority, Ratekeeper
+from common.kalman.ekf import EKF, SimpleSensor
+
+#vision point
+DIMSV = 2
+XV, SPEEDV = 0, 1
+VISION_POINT = 1
+
+class EKFV1D(EKF):
+  def __init__(self):
+    super(EKFV1D, self).__init__(False)
+    self.identity = np.matlib.identity(DIMSV)
+    self.state = np.matlib.zeros((DIMSV, 1))
+    self.var_init = 1e2   # ~ model variance when probability is 70%, so good starting point
+    self.covar = self.identity * self.var_init
+
+    # self.process_noise = np.asmatrix(np.diag([100, 10]))
+    self.process_noise = np.matlib.diag([0.5, 1])
+
+  def calc_transfer_fun(self, dt):
+    tf = np.matlib.identity(DIMSV)
+    tf[XV, SPEEDV] = dt
+    tfj = tf
+    return tf, tfj
+
+
+# nidec radar decoding
+def nidec_decode(cp, ar_pts):
+  for ii in cp.msgs_upd:
+    # filter points with very big distance, as fff (~255) is invalid. FIXME: use VAL tables from dbc
+    if cp.vl[ii]['LONG_DIST'] < 255:
+      ar_pts[ii] = [cp.vl[ii]['LONG_DIST'] + RDR_TO_LDR,
+                    -cp.vl[ii]['LAT_DIST'], cp.vl[ii]['REL_SPEED'], np.nan,
+                    cp.ts[ii], cp.vl[ii]['NEW_TRACK'], cp.ct[ii]]
+    elif ii in ar_pts:
+      del ar_pts[ii]
+  return ar_pts
+
+
+def _create_radard_can_parser():
+  dbc_f = 'acura_ilx_2016_nidec.dbc'
+  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)
+
+  return CANParser(dbc_f, signals, checks)
+
+
+# fuses camera and radar data for best lead detection
+def radard_thread(gctx=None):
+  set_realtime_priority(1)
+
+  context = zmq.Context()
+
+  # *** subscribe to features and model from visiond
+  model = messaging.sub_sock(context, service_list['model'].port)
+  logcan = messaging.sub_sock(context, service_list['can'].port)
+  live100 = messaging.sub_sock(context, service_list['live100'].port)
+
+  PP = PathPlanner(model)
+
+  # *** publish live20 and liveTracks
+  live20 = messaging.pub_sock(context, service_list['live20'].port)
+  liveTracks = messaging.pub_sock(context, service_list['liveTracks'].port)
+
+  # subscribe to stats about the car
+  # TODO: move this to new style packet
+  VP = VehicleParams(False)  # same for ILX and civic
+
+  ar_pts = {}
+  path_x = np.arange(0.0, 140.0, 0.1)    # 140 meters is max
+
+  # Time-alignment
+  rate = 20.   # model and radar are both at 20Hz
+  tsv = 1./rate
+  rdr_delay = 0.10   # radar data delay in s
+  v_len = 20         # how many speed data points to remember for t alignment with rdr data
+
+  enabled = 0
+  steer_angle = 0.
+
+  tracks = defaultdict(dict)
+
+  # Nidec
+  cp = _create_radard_can_parser()
+
+  # Kalman filter stuff: 
+  ekfv = EKFV1D()
+  speedSensorV = SimpleSensor(XV, 1, 2)
+
+  # v_ego
+  v_ego = None
+  v_ego_array = np.zeros([2, v_len])
+  v_ego_t_aligned = 0.
+
+  rk = Ratekeeper(rate, print_delay_threshold=np.inf)
+  while 1:
+    canMonoTimes = []
+    can_pub_radar = []
+    for a in messaging.drain_sock(logcan, wait_for_one=True):
+      canMonoTimes.append(a.logMonoTime)
+      can_pub_radar.extend(can_capnp_to_can_list_old(a, [1, 3]))
+
+    # only run on the 0x445 packets, used for timing
+    if not any(x[0] == 0x445 for x in can_pub_radar):
+      continue
+
+    cp.update_can(can_pub_radar)
+
+    if not cp.can_valid:
+      # TODO: handle this
+      pass
+
+    ar_pts = nidec_decode(cp, ar_pts)
+
+    # receive the live100s
+    l100 = messaging.recv_sock(live100)
+    if l100 is not None:
+      enabled = l100.live100.enabled
+      v_ego = l100.live100.vEgo
+      steer_angle = l100.live100.angleSteers
+
+      v_ego_array = np.append(v_ego_array, [[v_ego], [float(rk.frame)/rate]], 1)
+      v_ego_array = v_ego_array[:, 1:]
+
+    if v_ego is None:
+      continue
+
+    # *** get path prediction from the model ***
+    PP.update(sec_since_boot(), v_ego)
+
+    # 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())
+    else:
+      ekfv.state[XV] = PP.lead_dist
+      ekfv.covar = (np.diag([PP.lead_var, ekfv.var_init]))
+      ekfv.state[SPEEDV] = 0.
+      if VISION_POINT in ar_pts:
+        del ar_pts[VISION_POINT]
+
+    # *** compute the likely path_y ***
+    if enabled:    # use path from model path_poly
+      path_y = np.polyval(PP.d_poly, path_x)
+    else:          # use path from steer, set angle_offset to 0 since calibration does not exactly report the physical offset
+      path_y = calc_lookahead_offset(v_ego, steer_angle, path_x, VP, angle_offset=0)[0]
+
+    # *** remove missing points from meta data ***
+    for ids in tracks.keys():
+      if ids not in ar_pts:
+        tracks.pop(ids, None)
+
+    # *** compute the tracks ***
+    for ids in ar_pts:
+      # ignore the vision point for now
+      if ids == VISION_POINT:
+        continue
+      rpt = ar_pts[ids]
+
+      # align v_ego by a fixed time to align it with the radar measurement     
+      cur_time = float(rk.frame)/rate
+      v_ego_t_aligned = np.interp(cur_time - rdr_delay, v_ego_array[1], v_ego_array[0])
+      d_path = np.sqrt(np.amin((path_x - rpt[0]) ** 2 + (path_y - rpt[1]) ** 2))
+
+      # create the track
+      if ids not in tracks or rpt[5] == 1:
+        tracks[ids] = Track()
+      tracks[ids].update(rpt[0], rpt[1], rpt[2], d_path, v_ego_t_aligned)
+
+      # allow the vision model to remove the stationary flag if distance and rel speed roughly match
+      if VISION_POINT in ar_pts:
+        dist_to_vision = np.sqrt((0.5*(ar_pts[VISION_POINT][0] - rpt[0])) ** 2 + (2*(ar_pts[VISION_POINT][1] - rpt[1])) ** 2)
+        rel_speed_diff = abs(ar_pts[VISION_POINT][2] - rpt[2])
+        tracks[ids].mix_vision(dist_to_vision, rel_speed_diff)
+
+    # publish tracks (debugging)
+    dat = messaging.new_message()
+    dat.init('liveTracks', len(tracks))
+    for cnt, ids in enumerate(tracks.keys()):
+      dat.liveTracks[cnt].trackId = ids
+      dat.liveTracks[cnt].dRel = float(tracks[ids].dRel)
+      dat.liveTracks[cnt].yRel = float(tracks[ids].yRel)
+      dat.liveTracks[cnt].vRel = float(tracks[ids].vRel)
+      dat.liveTracks[cnt].aRel = float(tracks[ids].aRel)
+      dat.liveTracks[cnt].stationary = tracks[ids].stationary
+      dat.liveTracks[cnt].oncoming = tracks[ids].oncoming
+    liveTracks.send(dat.to_bytes())
+
+    idens = tracks.keys()
+    track_pts = np.array([tracks[iden].get_key_for_cluster() for iden in idens])
+
+    # If we have multiple points, cluster them
+    if len(track_pts) > 1:
+      link = linkage_vector(track_pts, method='centroid')
+      cluster_idxs = fcluster(link, 2.5, criterion='distance')
+      clusters = [None]*max(cluster_idxs)
+
+      for idx in xrange(len(track_pts)):
+        cluster_i = cluster_idxs[idx]-1
+
+        if clusters[cluster_i] == None:
+          clusters[cluster_i] = Cluster()
+        clusters[cluster_i].add(tracks[idens[idx]])
+    elif len(track_pts) == 1:
+      # TODO: why do we need this?
+      clusters = [Cluster()]
+      clusters[0].add(tracks[idens[0]])
+    else:
+      clusters = []
+
+    # *** extract the lead car ***
+    lead_clusters = [c for c in clusters
+                     if c.is_potential_lead(v_ego, enabled)]
+    lead_clusters.sort(key=lambda x: x.dRel)
+    lead_len = len(lead_clusters)
+
+    # *** extract the second lead from the whole set of leads ***
+    lead2_clusters = [c for c in lead_clusters
+                      if c.is_potential_lead2(lead_clusters)]
+    lead2_clusters.sort(key=lambda x: x.dRel)
+    lead2_len = len(lead2_clusters)
+
+    # *** publish live20 ***
+    dat = messaging.new_message()
+    dat.init('live20')
+    dat.live20.mdMonoTime = PP.logMonoTime
+    dat.live20.canMonoTimes = canMonoTimes
+    if lead_len > 0:
+      lead_clusters[0].toLive20(dat.live20.leadOne)
+      if lead2_len > 0:
+        lead2_clusters[0].toLive20(dat.live20.leadTwo)
+      else:
+        dat.live20.leadTwo.status = False
+    else:
+      dat.live20.leadOne.status = False
+
+    dat.live20.cumLagMs = -rk.remaining*1000.
+    live20.send(dat.to_bytes())
+
+    rk.monitor_time()
+
+def main(gctx=None):
+  radard_thread(gctx)
+
+if __name__ == "__main__":
+  main()

selfdrive/logcatd/Makefile

@@ -0,0 +1,58 @@
+CC = clang
+CXX = clang++
+
+PHONELIBS = ../../phonelibs
+
+WARN_FLAGS = -Werror=implicit-function-declaration \
+             -Werror=incompatible-pointer-types \
+             -Werror=int-conversion \
+             -Werror=return-type \
+             -Werror=format-extra-args
+
+CFLAGS = -std=gnu11 -g -fPIC -O2 $(WARN_FLAGS)
+CXXFLAGS = -std=c++11 -g -fPIC -O2 $(WARN_FLAGS)
+
+ZMQ_FLAGS = -I$(PHONELIBS)/zmq/aarch64/include
+ZMQ_LIBS = -L$(PHONELIBS)/zmq/aarch64/lib \
+           -l:libczmq.a -l:libzmq.a \
+           -lgnustl_shared
+
+CEREAL_FLAGS = -I$(PHONELIBS)/capnp-cpp/include
+CEREAL_LIBS = -L$(PHONELIBS)/capnp-cpp/aarch64/lib/ \
+              -l:libcapnp.a -l:libkj.a
+CEREAL_OBJS = ../../cereal/gen/c/log.capnp.o
+
+OBJS = logcatd.o \
+       log.capnp.o
+
+DEPS := $(OBJS:.o=.d)
+
+all: logcatd
+
+logcatd: $(OBJS)
+	@echo "[ LINK ] $@"
+	$(CXX) -fPIC -o '$@' $^ \
+            $(CEREAL_LIBS) \
+            $(ZMQ_LIBS) \
+            -llog
+
+%.o: %.cc
+	@echo "[ CXX ] $@"
+	$(CXX) $(CXXFLAGS) \
+           -I$(PHONELIBS)/android_system_core/include \
+           $(CEREAL_FLAGS) \
+           $(ZMQ_FLAGS) \
+           -I../ \
+           -I../../ \
+           -c -o '$@' '$<'
+
+log.capnp.o: ../../cereal/gen/cpp/log.capnp.c++
+	@echo "[ CXX ] $@"
+	$(CXX) $(CXXFLAGS) $(CEREAL_FLAGS) \
+           -c -o '$@' '$<'
+
+.PHONY: clean
+clean:
+	rm -f logcatd $(OBJS) $(DEPS)
+
+-include $(DEPS)

selfdrive/logcatd/logcatd.cc

@@ -0,0 +1,68 @@
+#include <cstdio>
+#include <cstdlib>
+#include <cassert>
+
+#include <log/log.h>
+#include <log/logger.h>
+#include <log/logprint.h>
+
+#include <zmq.h>
+#include <capnp/serialize.h>
+#include "common/timing.h"
+#include "cereal/gen/cpp/log.capnp.h"
+
+int main() {
+  int err;
+
+  struct logger_list *logger_list = android_logger_list_alloc(ANDROID_LOG_RDONLY, 0, 0);
+  assert(logger_list);
+  struct logger *main_logger = android_logger_open(logger_list, LOG_ID_MAIN);
+  assert(main_logger);
+  struct logger *radio_logger = android_logger_open(logger_list, LOG_ID_RADIO);
+  assert(radio_logger);
+  struct logger *system_logger = android_logger_open(logger_list, LOG_ID_SYSTEM);
+  assert(system_logger);
+  struct logger *crash_logger = android_logger_open(logger_list, LOG_ID_CRASH);
+  assert(crash_logger);
+  struct logger *kernel_logger = android_logger_open(logger_list, LOG_ID_KERNEL);
+  assert(kernel_logger);
+
+  void *context = zmq_ctx_new();
+  void *publisher = zmq_socket(context, ZMQ_PUB);
+  err = zmq_bind(publisher, "tcp://*:8020");
+  assert(err == 0);
+
+  while (1) {
+    log_msg log_msg;
+    err = android_logger_list_read(logger_list, &log_msg);
+    if (err <= 0) {
+      break;
+    }
+
+    AndroidLogEntry entry;
+    err = android_log_processLogBuffer(&log_msg.entry_v1, &entry);
+    if (err < 0) {
+      continue;
+    }
+
+    capnp::MallocMessageBuilder msg;
+    cereal::Event::Builder event = msg.initRoot<cereal::Event>();
+    event.setLogMonoTime(nanos_since_boot());
+    auto androidEntry = event.initAndroidLogEntry();
+    androidEntry.setId(log_msg.id());
+    androidEntry.setTs(entry.tv_sec * 1000000000ULL + entry.tv_nsec);
+    androidEntry.setPriority(entry.priority);
+    androidEntry.setPid(entry.pid);
+    androidEntry.setTid(entry.tid);
+    androidEntry.setTag(entry.tag);
+    androidEntry.setMessage(entry.message);
+
+    auto words = capnp::messageToFlatArray(msg);
+    auto bytes = words.asBytes();
+    zmq_send(publisher, bytes.begin(), bytes.size(), 0);
+  }
+
+  android_logger_list_close(logger_list);
+
+  return 0;
+}

selfdrive/loggerd/config.py

@@ -0,0 +1,9 @@
+import os
+
+# fetch from environment
+DONGLE_ID = os.getenv("DONGLE_ID")
+DONGLE_SECRET = os.getenv("DONGLE_SECRET")
+
+ROOT = '/sdcard/realdata/'
+
+SEGMENT_LENGTH = 60

selfdrive/loggerd/logger.py

@@ -0,0 +1,65 @@
+import os
+import time
+
+
+class Logger(object):
+  def __init__(self, root, init_data):
+    self.root = root
+    self.init_data = init_data
+
+    self.part = None
+    self.data_dir = None
+    self.cur_dir = None
+    self.log_file = None
+    self.started = False
+    self.log_path = None
+    self.lock_path = None
+    self.log_file = None
+
+  def open(self):
+    self.data_dir = self.cur_dir + "--" + str(self.part)
+
+    try:
+      os.makedirs(self.data_dir)
+    except OSError:
+      pass
+
+    self.log_path = os.path.join(self.data_dir, "rlog")
+    self.lock_path = self.log_path + ".lock"
+
+    open(self.lock_path, "wb").close()
+    self.log_file = open(self.log_path, "wb")
+    self.log_file.write(self.init_data)
+
+  def start(self):
+    self.part = 0
+    self.cur_dir = self.root + time.strftime("%Y-%m-%d--%H-%M-%S")
+
+    self.open()
+
+    self.started = True
+
+    return self.data_dir, self.part
+
+  def stop(self):
+    if not self.started:
+      return
+    self.log_file.close()
+    os.unlink(self.lock_path)
+    self.started = False
+
+  def rotate(self):
+    old_lock_path = self.lock_path
+    old_log_file = self.log_file
+    self.part += 1
+    self.open()
+
+    old_log_file.close()
+    os.unlink(old_lock_path)
+
+    return self.data_dir, self.part
+
+  def log_data(self, d):
+    if not self.started:
+      return
+    self.log_file.write(d)

selfdrive/loggerd/loggerd.py

@@ -0,0 +1,86 @@
+#!/usr/bin/env python
+import os
+import json
+import zmq
+
+import common.realtime as realtime
+from common.services import service_list
+from selfdrive.swaglog import cloudlog
+import selfdrive.messaging as messaging
+
+import uploader
+from logger import Logger
+
+from selfdrive.loggerd.config import ROOT, SEGMENT_LENGTH
+
+
+def gen_init_data(gctx):
+  msg = messaging.new_message()
+
+  kernel_args = open("/proc/cmdline", "r").read().strip().split(" ")
+  msg.initData.kernelArgs = kernel_args
+
+  msg.initData.gctx = json.dumps(gctx)
+  if os.getenv('DONGLE_ID'):
+    msg.initData.dongleId = os.getenv('DONGLE_ID')
+
+  return msg.to_bytes()
+
+def main(gctx=None):
+  logger = Logger(ROOT, gen_init_data(gctx))
+
+  context = zmq.Context()
+  poller = zmq.Poller()
+
+  # we push messages to visiond to rotate image recordings
+  vision_control_sock = context.socket(zmq.PUSH)
+  vision_control_sock.connect("tcp://127.0.0.1:8001")
+
+  # register listeners for all services
+  for service in service_list.itervalues():
+    if service.should_log and service.port is not None:
+      messaging.sub_sock(context, service.port, poller)
+
+  uploader.clear_locks(ROOT)
+
+  cur_dir, cur_part = logger.start()
+  try:
+    cloudlog.info("starting in dir %r", cur_dir)
+
+    rotate_msg = messaging.log.LogRotate.new_message()
+    rotate_msg.segmentNum = cur_part
+    rotate_msg.path = cur_dir
+    vision_control_sock.send(rotate_msg.to_bytes())
+
+    last_rotate = realtime.sec_since_boot()
+    while True:
+      polld = poller.poll(timeout=1000)
+      for sock, mode in polld:
+        if mode != zmq.POLLIN:
+          continue
+        dat = sock.recv()
+
+        # print "got", len(dat), realtime.sec_since_boot()
+        # logevent = log_capnp.Event.from_bytes(dat)
+        # print str(logevent)
+        logger.log_data(dat)
+
+      t = realtime.sec_since_boot()
+      if (t - last_rotate) > SEGMENT_LENGTH:
+        last_rotate += SEGMENT_LENGTH
+
+        cur_dir, cur_part = logger.rotate()
+        cloudlog.info("rotated to %r", cur_dir)
+
+        rotate_msg = messaging.log.LogRotate.new_message()
+        rotate_msg.segmentNum = cur_part
+        rotate_msg.path = cur_dir
+
+        vision_control_sock.send(rotate_msg.to_bytes())
+
+  finally:
+    logger.stop()
+
+if __name__ == "__main__":
+  main()
+

selfdrive/loggerd/uploader.py

@@ -0,0 +1,238 @@
+#!/usr/bin/env python
+import os
+import time
+import stat
+import random
+import ctypes
+import inspect
+import requests
+import traceback
+import threading
+
+from selfdrive.swaglog import cloudlog
+from selfdrive.loggerd.config import DONGLE_ID, DONGLE_SECRET, ROOT
+
+from common.api import api_get
+
+def raise_on_thread(t, exctype):
+  for ctid, tobj in threading._active.items():
+    if tobj is t:
+      tid = ctid
+      break
+  else:
+    raise Exception("Could not find thread")
+
+  '''Raises an exception in the threads with id tid'''
+  if not inspect.isclass(exctype):
+    raise TypeError("Only types can be raised (not instances)")
+
+  res = ctypes.pythonapi.PyThreadState_SetAsyncExc(ctypes.c_long(tid),
+                                                   ctypes.py_object(exctype))
+  if res == 0:
+    raise ValueError("invalid thread id")
+  elif res != 1:
+    # "if it returns a number greater than one, you're in trouble,
+    # and you should call it again with exc=NULL to revert the effect"
+    ctypes.pythonapi.PyThreadState_SetAsyncExc(tid, 0)
+    raise SystemError("PyThreadState_SetAsyncExc failed")
+
+def listdir_with_creation_date(d):
+  lst = os.listdir(d)
+  for fn in lst:
+    try:
+      st = os.stat(os.path.join(d, fn))
+      ctime = st[stat.ST_CTIME]
+      yield (ctime, fn)
+    except OSError:
+      cloudlog.exception("listdir_with_creation_date: stat failed?")
+      yield (None, fn)
+
+def listdir_by_creation_date(d):
+  times_and_paths = list(listdir_with_creation_date(d))
+  return [path for _, path in sorted(times_and_paths)]
+
+def clear_locks(root):
+  for logname in os.listdir(root):
+    path = os.path.join(root, logname)
+    try:
+      for fname in os.listdir(path):
+        if fname.endswith(".lock"):
+          os.unlink(os.path.join(path, fname))
+    except OSError:
+      cloudlog.exception("clear_locks failed")
+
+
+class Uploader(object):
+  def __init__(self, dongle_id, dongle_secret, root):
+    self.dongle_id = dongle_id
+    self.dongle_secret = dongle_secret
+    self.root = root
+
+    self.upload_thread = None
+
+    self.last_resp = None
+    self.last_exc = None
+
+  def clean_dirs(self):
+    try:
+      for logname in os.listdir(self.root):
+        path = os.path.join(self.root, logname)
+        # remove empty directories
+        if not os.listdir(path):
+          os.rmdir(path)
+    except OSError:
+      cloudlog.exception("clean_dirs failed")
+
+  def gen_upload_files(self):
+    for logname in listdir_by_creation_date(self.root):
+      path = os.path.join(self.root, logname)
+      names = os.listdir(path)
+      if any(name.endswith(".lock") for name in names):
+        continue
+
+      for name in names:
+        key = os.path.join(logname, name)
+        fn = os.path.join(path, name)
+
+        yield (name, key, fn)
+
+  def next_file_to_upload(self):
+    # try to upload log files first
+    for name, key, fn in self.gen_upload_files():
+      if name in ["rlog", "rlog.bz2"]:
+        return (key, fn, 0)
+
+    # then upload camera files no not on wifi
+    for name, key, fn in self.gen_upload_files():
+      if not name.endswith('.lock') and not name.endswith(".tmp"):
+        return (key, fn, 1)
+
+    return None
+
+
+  def do_upload(self, key, fn):
+    try:
+      url_resp = api_get("upload_url", timeout=2,
+                         id=self.dongle_id, secret=self.dongle_secret,
+                         path=key)
+      url = url_resp.text
+      cloudlog.info({"upload_url", url})
+
+      with open(fn, "rb") as f:
+        self.last_resp = requests.put(url, data=f)
+    except Exception as e:
+      self.last_exc = (e, traceback.format_exc())
+      raise
+
+  def normal_upload(self, key, fn):
+    self.last_resp = None
+    self.last_exc = None
+
+    try:
+      self.do_upload(key, fn)
+    except Exception:
+      pass
+
+    return self.last_resp
+
+  def killable_upload(self, key, fn):
+      self.last_resp = None
+      self.last_exc = None
+
+      self.upload_thread = threading.Thread(target=lambda: self.do_upload(key, fn))
+      self.upload_thread.start()
+      self.upload_thread.join()
+      self.upload_thread = None
+
+      return self.last_resp
+
+  def abort_upload(self):
+    thread = self.upload_thread
+    if thread is None:
+      return
+    if not thread.is_alive():
+      return
+    raise_on_thread(thread, SystemExit)
+    thread.join()
+
+  def upload(self, key, fn):
+    # write out the bz2 compress
+    if fn.endswith("log"):
+      ext = ".bz2"
+      cloudlog.info("compressing %r to %r", fn, fn+ext)
+      if os.system("nice -n 19 bzip2 -c %s > %s.tmp && mv %s.tmp %s%s && rm %s" % (fn, fn, fn, fn, ext, fn)) != 0:
+        cloudlog.exception("upload: bzip2 compression failed")
+        return False
+
+      # assuming file is named properly
+      key += ext
+      fn += ext
+
+    try:
+      sz = os.path.getsize(fn)
+    except OSError:
+      cloudlog.exception("upload: getsize failed")
+      return False
+
+    cloudlog.event("upload", key=key, fn=fn, sz=sz)
+
+    cloudlog.info("checking %r with size %r", key, sz)
+
+    if sz == 0:
+      # can't upload files of 0 size
+      os.unlink(fn) # delete the file
+      success = True
+    else:
+      cloudlog.info("uploading %r", fn)
+      # stat = self.killable_upload(key, fn)
+      stat = self.normal_upload(key, fn)
+      if stat is not None and stat.status_code == 200:
+        cloudlog.event("upload_success", key=key, fn=fn, sz=sz)
+        os.unlink(fn) # delete the file
+        success = True
+      else:
+        cloudlog.event("upload_failed", stat=stat, exc=self.last_exc, key=key, fn=fn, sz=sz)
+        success = False
+
+    self.clean_dirs()
+
+    return success
+
+
+
+def uploader_fn(exit_event):
+  cloudlog.info("uploader_fn")
+
+  uploader = Uploader(DONGLE_ID, DONGLE_SECRET, ROOT)
+
+  while True:
+    backoff = 0.1
+    while True:
+
+      if exit_event.is_set():
+        return
+
+      d = uploader.next_file_to_upload()
+      if d is None:
+        break
+
+      key, fn, _ = d
+
+      cloudlog.info("to upload %r", d)
+      success = uploader.upload(key, fn)
+      if success:
+        backoff = 0.1
+      else:
+        cloudlog.info("backoff %r", backoff)
+        time.sleep(backoff + random.uniform(0, backoff))
+        backoff *= 2
+      cloudlog.info("upload done, success=%r", success)
+
+    time.sleep(5)
+
+def main(gctx=None):
+  uploader_fn(threading.Event())
+
+if __name__ == "__main__":
+  main()
+

selfdrive/logmessaged.py

@@ -0,0 +1,39 @@
+#!/usr/bin/env python
+import zmq
+from logentries import LogentriesHandler
+from common.services import service_list
+import selfdrive.messaging as messaging
+
+def main(gctx):
+  # setup logentries. we forward log messages to it
+  le_token = "bc65354a-b887-4ef4-8525-15dd51230e8c"
+  le_handler = LogentriesHandler(le_token, use_tls=False)
+
+  le_level = 20 #logging.INFO
+
+  ctx = zmq.Context()
+  sock = ctx.socket(zmq.PULL)
+  sock.bind("ipc:///tmp/logmessage")
+
+  # and we publish them
+  pub_sock = messaging.pub_sock(ctx, service_list['logMessage'].port)
+
+  while True:
+    dat = ''.join(sock.recv_multipart())
+
+    # print "RECV", repr(dat)
+
+    levelnum = ord(dat[0])
+    dat = dat[1:]
+
+    if levelnum >= le_level:
+      # push to logentries
+      le_handler.emit_raw(dat)
+
+    # then we publish them
+    msg = messaging.new_message()
+    msg.logMessage = dat
+    pub_sock.send(msg.to_bytes())
+
+if __name__ == "__main__":
+  main(None)

selfdrive/manager.py

@@ -0,0 +1,278 @@
+#!/usr/bin/env python
+import os
+import sys
+import time
+import importlib
+import subprocess
+import signal
+import traceback
+import usb1
+from multiprocessing import Process
+from common.services import service_list
+
+import zmq
+
+from setproctitle import setproctitle
+
+from selfdrive.swaglog import cloudlog
+import selfdrive.messaging as messaging
+from selfdrive.thermal import read_thermal
+from selfdrive.registration import register
+
+import common.crash
+
+# comment out anything you don't want to run
+managed_processes = {
+  "uploader": "selfdrive.loggerd.uploader",
+  "controlsd": "selfdrive.controls.controlsd",
+  "radard": "selfdrive.controls.radard",
+  "calibrationd": "selfdrive.calibrationd.calibrationd",
+  "loggerd": "selfdrive.loggerd.loggerd",
+  "logmessaged": "selfdrive.logmessaged",
+  #"boardd": "selfdrive.boardd.boardd",
+  "logcatd": ("logcatd", ["./logcatd"]),
+  "boardd": ("boardd", ["./boardd"]),   # switch to c++ boardd
+  "ui": ("ui", ["./ui"]),
+  "visiond": ("visiond", ["./visiond"]),
+  "sensord": ("sensord", ["./sensord"]), }
+
+running = {}
+
+car_started_processes = ['controlsd', 'loggerd', 'visiond', 'sensord', 'radard', 'calibrationd']
+
+
+# ****************** process management functions ******************
+def launcher(proc, gctx):
+  try:
+    # unset the signals
+    signal.signal(signal.SIGINT, signal.SIG_DFL)
+    signal.signal(signal.SIGTERM, signal.SIG_DFL)
+
+    # import the process
+    mod = importlib.import_module(proc)
+
+    # rename the process
+    setproctitle(proc)
+
+    # exec the process
+    mod.main(gctx)
+  except Exception:
+    # can't install the crash handler becuase sys.excepthook doesn't play nice
+    # with threads, so catch it here.
+    common.crash.capture_exception()
+    raise
+
+def nativelauncher(pargs, cwd):
+  # exec the process
+  os.chdir(cwd)
+
+  # because when extracted from pex zips permissions get lost -_-
+  os.chmod(pargs[0], 0o700)
+
+  os.execvp(pargs[0], pargs)
+
+def start_managed_process(name):
+  if name in running or name not in managed_processes:
+    return
+  proc = managed_processes[name]
+  if isinstance(proc, basestring):
+    cloudlog.info("starting python %s" % proc)
+    running[name] = Process(name=name, target=launcher, args=(proc, gctx))
+  else:
+    pdir, pargs = proc
+    cwd = os.path.dirname(os.path.realpath(__file__))
+    if pdir is not None:
+      cwd = os.path.join(cwd, pdir)
+    cloudlog.info("starting process %s" % name)
+    running[name] = Process(name=name, target=nativelauncher, args=(pargs, cwd))
+  running[name].start()
+
+def kill_managed_process(name):
+  if name not in running or name not in managed_processes:
+    return
+  cloudlog.info("killing %s" % name)
+  running[name].terminate()
+  running[name].join(5.0)
+  if running[name].exitcode is None:
+    cloudlog.info("killing %s with SIGKILL" % name)
+    os.kill(running[name].pid, signal.SIGKILL)
+    running[name].join()
+    cloudlog.info("%s is finally dead" % name)
+  else:
+    cloudlog.info("%s is dead with %d" % (name, running[name].exitcode))
+  del running[name]
+
+def cleanup_all_processes(signal, frame):
+  cloudlog.info("caught ctrl-c %s %s" % (signal, frame))
+  for name in running.keys():
+    kill_managed_process(name)
+  sys.exit(0)
+
+# ****************** run loop ******************
+
+def manager_init():
+  global gctx
+
+  reg_res = register()
+  if reg_res:
+    dongle_id, dongle_secret = reg_res
+  else:
+    raise Exception("server registration failed")
+
+  # set dongle id
+  cloudlog.info("dongle id is " + dongle_id)
+  os.environ['DONGLE_ID'] = dongle_id
+  os.environ['DONGLE_SECRET'] = dongle_secret
+
+  cloudlog.bind_global(dongle_id=dongle_id)
+
+  # set gctx
+  gctx = {
+    "calibration": {
+      "initial_homography": [1.15728010e+00, -4.69379619e-02, 7.46450623e+01,
+                             7.99253014e-02, 1.06372458e+00, 5.77762553e+01,
+                             9.35543519e-05, -1.65429898e-04, 9.98062699e-01]
+    }
+  }
+
+  # hook to kill all processes
+  signal.signal(signal.SIGINT, cleanup_all_processes)
+  signal.signal(signal.SIGTERM, cleanup_all_processes)
+
+def manager_thread():
+  # now loop
+  context = zmq.Context()
+  thermal_sock = messaging.pub_sock(context, service_list['thermal'].port)
+  health_sock = messaging.sub_sock(context, service_list['health'].port)
+
+  cloudlog.info("manager start")
+
+  start_managed_process("logmessaged")
+  start_managed_process("logcatd")
+  start_managed_process("uploader")
+  start_managed_process("ui")
+
+  # *** wait for the board ***
+  wait_for_device()
+
+  # flash the device
+  if os.getenv("NOPROG") is None:
+    boarddir = os.path.dirname(os.path.abspath(__file__))+"/../board/"
+    os.system("cd %s && make" % boarddir)
+
+  start_managed_process("boardd")
+
+  if os.getenv("STARTALL") is not None:
+    for p in car_started_processes:
+      start_managed_process(p)
+
+  while 1:
+    # get health of board, log this in "thermal"
+    td = messaging.recv_sock(health_sock, wait=True)
+    print td
+
+    # replace thermald
+    msg = read_thermal()
+    thermal_sock.send(msg.to_bytes())
+    print msg
+
+    # TODO: add car battery voltage check
+    max_temp = max(msg.thermal.cpu0, msg.thermal.cpu1,
+                   msg.thermal.cpu2, msg.thermal.cpu3) / 10.0
+
+    # uploader is gated based on the phone temperature
+    if max_temp > 85.0:
+      cloudlog.info("over temp: %r", max_temp)
+      kill_managed_process("uploader")
+    elif max_temp < 70.0:
+      start_managed_process("uploader")
+
+    # start constellation of processes when the car starts
+    if td.health.started:
+      for p in car_started_processes:
+        start_managed_process(p)
+    else:
+      for p in car_started_processes:
+        kill_managed_process(p)
+
+    # check the status of all processes, did any of them die?
+    for p in running:
+      cloudlog.info("   running %s %s" % (p, running[p]))
+
+
+# optional, build the c++ binaries and preimport the python for speed
+def manager_prepare():
+  for p in managed_processes:
+    proc = managed_processes[p]
+    if isinstance(proc, basestring):
+      # import this python
+      cloudlog.info("preimporting %s" % proc)
+      importlib.import_module(proc)
+    else:
+      # build this process
+      cloudlog.info("building %s" % (proc,))
+      try:
+        subprocess.check_call(["make", "-j4"], cwd=proc[0])
+      except subprocess.CalledProcessError:
+        # make clean if the build failed
+        cloudlog.info("building %s failed, make clean" % (proc, ))
+        subprocess.check_call(["make", "clean"], cwd=proc[0])
+        subprocess.check_call(["make", "-j4"], cwd=proc[0])
+
+def manager_test():
+  global managed_processes
+  managed_processes = {}
+  managed_processes["test1"] = ("test", ["./test.py"])
+  managed_processes["test2"] = ("test", ["./test.py"])
+  managed_processes["test3"] = "selfdrive.test.test"
+  manager_prepare()
+  start_managed_process("test1")
+  start_managed_process("test2")
+  start_managed_process("test3")
+  print running
+  time.sleep(3)
+  kill_managed_process("test1")
+  kill_managed_process("test2")
+  kill_managed_process("test3")
+  print running
+  time.sleep(10)
+
+def wait_for_device():
+  while 1:
+    try:
+      context = usb1.USBContext()
+      for device in context.getDeviceList(skip_on_error=True):
+        if (device.getVendorID() == 0xbbaa and device.getProductID() == 0xddcc) or \
+           (device.getVendorID() == 0x0483 and device.getProductID() == 0xdf11):
+          handle = device.open()
+          handle.claimInterface(0)
+          cloudlog.info("found board")
+          handle.close()
+          return
+    except Exception as e:
+      print "exception", e,
+    print "waiting..."
+    time.sleep(1)
+
+def main():
+  if os.getenv("NOLOG") is not None:
+    del managed_processes['loggerd']
+  if os.getenv("NOBOARD") is not None:
+    del managed_processes['boardd']
+
+  manager_init()
+
+  if len(sys.argv) > 1 and sys.argv[1] == "test":
+    manager_test()
+  else:
+    manager_prepare()
+    try:
+      manager_thread()
+    except Exception:
+      traceback.print_exc()
+      common.crash.capture_exception()
+    finally:
+      cleanup_all_processes(None, None)
+
+if __name__ == "__main__":
+  main()

selfdrive/messaging.py

@@ -0,0 +1,52 @@
+import zmq
+
+from cereal import log
+from common import realtime
+
+def new_message():
+  dat = log.Event.new_message()
+  dat.logMonoTime = int(realtime.sec_since_boot() * 1e9)
+  return dat
+
+def pub_sock(context, port, addr="*"):
+  sock = context.socket(zmq.PUB)
+  sock.bind("tcp://%s:%d" % (addr, port))
+  return sock
+
+def sub_sock(context, port, poller=None, addr="127.0.0.1"):
+  sock = context.socket(zmq.SUB)
+  sock.connect("tcp://%s:%d" % (addr, port))
+  sock.setsockopt(zmq.SUBSCRIBE, "")
+  if poller is not None:
+    poller.register(sock, zmq.POLLIN)
+  return sock
+
+def drain_sock(sock, wait_for_one=False):
+  ret = []
+  while 1:
+    try:
+      if wait_for_one and len(ret) == 0:
+        dat = sock.recv()
+      else:
+        dat = sock.recv(zmq.NOBLOCK)
+      dat = log.Event.from_bytes(dat)
+      ret.append(dat)
+    except zmq.error.Again:
+      break
+  return ret
+
+
+# TODO: print when we drop packets?
+def recv_sock(sock, wait=False):
+  dat = None
+  while 1:
+    try:
+      if wait and dat is None:
+        dat = sock.recv()
+      else:
+        dat = sock.recv(zmq.NOBLOCK)
+    except zmq.error.Again:
+      break
+  if dat is not None:
+    dat = log.Event.from_bytes(dat)
+  return dat

selfdrive/registration.py

@@ -0,0 +1,38 @@
+import os
+import json
+import subprocess
+
+from selfdrive.swaglog import cloudlog
+from common.api import api_get
+
+DONGLEAUTH_PATH = "/sdcard/dongleauth"
+
+def get_imei():
+  # Telephony.getDeviceId()
+  result = subprocess.check_output(["service", "call", "phone", "130"]).strip().split("\n")
+  hex_data = ''.join(l[14:49] for l in result[1:]).replace(" ", "")
+  data = hex_data.decode("hex")
+
+  imei_str = data[8:-4].replace("\x00", "")
+  return imei_str
+
+def get_serial():
+  return subprocess.check_output(["getprop", "ro.serialno"]).strip()
+
+def register():
+  try:
+    if os.path.exists(DONGLEAUTH_PATH):
+      dongleauth = json.load(open(DONGLEAUTH_PATH))
+    else:
+      resp = api_get("pilot_auth", method='POST', imei=get_imei(), serial=get_serial())
+      resp = resp.text
+      dongleauth = json.loads(resp)
+      open(DONGLEAUTH_PATH, "w").write(resp)
+    return dongleauth["dongle_id"], dongleauth["dongle_secret"]
+  except Exception:
+    cloudlog.exception("failed to authenticate")
+    return None
+
+if __name__ == "__main__":
+  print api_get("").text
+  print register()

selfdrive/sensord/Makefile

@@ -0,0 +1,60 @@
+CC = clang
+CXX = clang++
+
+PHONELIBS = ../../phonelibs
+
+WARN_FLAGS = -Werror=implicit-function-declaration \
+             -Werror=incompatible-pointer-types \
+             -Werror=int-conversion \
+             -Werror=return-type \
+             -Werror=format-extra-args
+
+CFLAGS = -std=gnu11 -g -fPIC -O2 $(WARN_FLAGS)
+CXXFLAGS = -std=c++11 -g -fPIC -O2 $(WARN_FLAGS)
+
+ZMQ_FLAGS = -I$(PHONELIBS)/zmq/aarch64/include
+ZMQ_LIBS = -L$(PHONELIBS)/zmq/aarch64/lib \
+           -l:libczmq.a -l:libzmq.a \
+           -lgnustl_shared
+
+CEREAL_FLAGS = -I$(PHONELIBS)/capnp-cpp/include
+CEREAL_LIBS = -L$(PHONELIBS)/capnp-cpp/aarch64/lib/ \
+              -l:libcapnp.a -l:libkj.a
+CEREAL_OBJS = ../../cereal/gen/c/log.capnp.o
+
+OBJS = sensors.o \
+       log.capnp.o
+
+DEPS := $(OBJS:.o=.d)
+
+all: sensord
+
+sensord: $(OBJS)
+	@echo "[ LINK ] $@"
+	$(CXX) -fPIC -o '$@' $^ \
+            $(CEREAL_LIBS) \
+            $(ZMQ_LIBS) \
+            -lhardware
+
+sensors.o: sensors.cc
+	@echo "[ CXX ] $@"
+	$(CXX) $(CXXFLAGS) \
+           -I$(PHONELIBS)/android_system_core/include \
+           $(CEREAL_FLAGS) \
+           $(ZMQ_FLAGS) \
+           -I../ \
+           -I../../ \
+           -c -o '$@' '$<'
+
+
+log.capnp.o: ../../cereal/gen/cpp/log.capnp.c++
+	@echo "[ CXX ] $@"
+	$(CXX) $(CXXFLAGS) $(CEREAL_FLAGS) \
+           -c -o '$@' '$<'
+  
+
+.PHONY: clean
+clean:
+	rm -f sensord $(OBJS) $(DEPS)
+
+-include $(DEPS)

selfdrive/sensord/sensors.cc

@@ -0,0 +1,227 @@
+#include <stdio.h>
+#include <stdint.h>
+#include <string.h>
+#include <signal.h>
+#include <unistd.h>
+#include <sys/time.h>
+#include <sys/cdefs.h>
+#include <sys/types.h>
+
+#include <pthread.h>
+
+#include <cutils/log.h>
+
+#include <hardware/gps.h>
+#include <hardware/sensors.h>
+#include <utils/Timers.h>
+
+#include <zmq.h>
+
+#include <capnp/serialize.h>
+
+#include "common/timing.h"
+
+#include "cereal/gen/cpp/log.capnp.h"
+
+// zmq output
+static void *gps_publisher;
+
+#define SENSOR_ACCELEROMETER 1
+#define SENSOR_MAGNETOMETER 2
+#define SENSOR_GYRO 4
+
+void sensor_loop() {
+  printf("*** sensor loop\n");
+  struct sensors_poll_device_t* device;
+  struct sensors_module_t* module;
+
+  hw_get_module(SENSORS_HARDWARE_MODULE_ID, (hw_module_t const**)&module);
+  sensors_open(&module->common, &device);
+
+  // required
+  struct sensor_t const* list;
+  int count = module->get_sensors_list(module, &list);
+  printf("%d sensors found\n", count);
+
+  device->activate(device, SENSOR_ACCELEROMETER, 0);
+  device->activate(device, SENSOR_MAGNETOMETER, 0);
+  device->activate(device, SENSOR_GYRO, 0);
+
+  device->activate(device, SENSOR_ACCELEROMETER, 1);
+  device->activate(device, SENSOR_MAGNETOMETER, 1);
+  device->activate(device, SENSOR_GYRO, 1);
+
+  device->setDelay(device, SENSOR_ACCELEROMETER, ms2ns(10));
+  device->setDelay(device, SENSOR_GYRO, ms2ns(10));
+  device->setDelay(device, SENSOR_MAGNETOMETER, ms2ns(100));
+
+  static const size_t numEvents = 16;
+  sensors_event_t buffer[numEvents];
+
+  // zmq output
+  void *context = zmq_ctx_new();
+  void *publisher = zmq_socket(context, ZMQ_PUB);
+  zmq_bind(publisher, "tcp://*:8003");
+
+  while (1) {
+    int n = device->poll(device, buffer, numEvents);
+    if (n == 0) continue;
+    if (n < 0) {
+      printf("sensor_loop poll failed: %d\n", n);
+      continue;
+    }
+
+    uint64_t log_time = nanos_since_boot();
+
+    capnp::MallocMessageBuilder msg;
+    cereal::Event::Builder event = msg.initRoot<cereal::Event>();
+    event.setLogMonoTime(log_time);
+
+    auto sensorEvents = event.initSensorEvents(n);
+
+    for (int i = 0; i < n; i++) {
+
+      const sensors_event_t& data = buffer[i];
+
+      sensorEvents[i].setVersion(data.version);
+      sensorEvents[i].setSensor(data.sensor);
+      sensorEvents[i].setType(data.type);
+      sensorEvents[i].setTimestamp(data.timestamp);
+
+      switch (data.type) {
+      case SENSOR_TYPE_ACCELEROMETER: {
+        auto svec = sensorEvents[i].initAcceleration();
+        kj::ArrayPtr<const float> vs(&data.acceleration.v[0], 3);
+        svec.setV(vs);
+        svec.setStatus(data.acceleration.status);
+        break;
+      }
+      case SENSOR_TYPE_MAGNETIC_FIELD: {
+        auto svec = sensorEvents[i].initMagnetic();
+        kj::ArrayPtr<const float> vs(&data.magnetic.v[0], 3);
+        svec.setV(vs);
+        svec.setStatus(data.magnetic.status);
+        break;
+      }
+      case SENSOR_TYPE_GYROSCOPE: {
+        auto svec = sensorEvents[i].initGyro();
+        kj::ArrayPtr<const float> vs(&data.gyro.v[0], 3);
+        svec.setV(vs);
+        svec.setStatus(data.gyro.status);
+        break;
+      }
+      default:
+        continue;
+      }
+    }
+
+    auto words = capnp::messageToFlatArray(msg);
+    auto bytes = words.asBytes();
+    // printf("send %d\n", bytes.size());
+    zmq_send(publisher, bytes.begin(), bytes.size(), 0);
+
+  }
+}
+
+static const GpsInterface* gGpsInterface = NULL;
+static const AGpsInterface* gAGpsInterface = NULL;
+static const GpsMeasurementInterface* gGpsMeasurementInterface = NULL;
+
+static void nmea_callback(GpsUtcTime timestamp, const char* nmea, int length) {
+
+  uint64_t log_time = nanos_since_boot();
+  uint64_t log_time_wall = nanos_since_epoch();
+
+  capnp::MallocMessageBuilder msg;
+  cereal::Event::Builder event = msg.initRoot<cereal::Event>();
+  event.setLogMonoTime(log_time);
+
+  auto nmeaData = event.initGpsNMEA();
+  nmeaData.setTimestamp(timestamp);
+  nmeaData.setLocalWallTime(log_time_wall);
+  nmeaData.setNmea(nmea);
+
+  auto words = capnp::messageToFlatArray(msg);
+  auto bytes = words.asBytes();
+  // printf("gps send %d\n", bytes.size());
+  zmq_send(gps_publisher, bytes.begin(), bytes.size(), 0);
+}
+
+static pthread_t create_thread_callback(const char* name, void (*start)(void *), void* arg) {
+  printf("creating thread: %s\n", name);
+  pthread_t thread;
+  pthread_attr_t attr;
+  int err;
+
+  err = pthread_attr_init(&attr);
+  err = pthread_create(&thread, &attr, (void*(*)(void*))start, arg);
+
+  return thread;
+}
+
+static GpsCallbacks gps_callbacks = {
+  sizeof(GpsCallbacks),
+  NULL,
+  NULL,
+  NULL,
+  nmea_callback,
+  NULL,
+  NULL,
+  NULL,
+  create_thread_callback,
+};
+
+static void agps_status_cb(AGpsStatus *status) {
+  switch (status->status) {
+    case GPS_REQUEST_AGPS_DATA_CONN:
+      fprintf(stdout, "*** data_conn_open\n");
+      gAGpsInterface->data_conn_open("internet");
+      break;
+    case GPS_RELEASE_AGPS_DATA_CONN:
+      fprintf(stdout, "*** data_conn_closed\n");
+      gAGpsInterface->data_conn_closed();
+      break;
+  }
+}
+
+static AGpsCallbacks agps_callbacks = {
+  agps_status_cb,
+  create_thread_callback,
+};
+
+
+
+static void gps_init() {
+  printf("*** init GPS\n");
+  hw_module_t* module;
+  hw_get_module(GPS_HARDWARE_MODULE_ID, (hw_module_t const**)&module);
+
+  hw_device_t* device;
+  module->methods->open(module, GPS_HARDWARE_MODULE_ID, &device);
+
+  // ** get gps interface **
+  gps_device_t* gps_device = (gps_device_t *)device;
+  gGpsInterface = gps_device->get_gps_interface(gps_device);
+  gAGpsInterface = (const AGpsInterface*)gGpsInterface->get_extension(AGPS_INTERFACE);
+
+
+
+  gGpsInterface->init(&gps_callbacks);
+  gAGpsInterface->init(&agps_callbacks);
+  gAGpsInterface->set_server(AGPS_TYPE_SUPL, "supl.google.com", 7276);
+
+  gGpsInterface->delete_aiding_data(GPS_DELETE_ALL);
+  gGpsInterface->start();
+  gGpsInterface->set_position_mode(GPS_POSITION_MODE_MS_BASED,
+                                   GPS_POSITION_RECURRENCE_PERIODIC,
+                                   1000, 0, 0);
+  void *gps_context = zmq_ctx_new();
+  gps_publisher = zmq_socket(gps_context, ZMQ_PUB);
+  zmq_bind(gps_publisher, "tcp://*:8004");
+}
+
+int main(int argc, char *argv[]) {
+  gps_init();
+  sensor_loop();
+}
+

selfdrive/swaglog.py

@@ -0,0 +1,38 @@
+import os
+import logging
+
+import zmq
+
+from common.logging_extra import SwagLogger, SwagFormatter
+
+class LogMessageHandler(logging.Handler):
+  def __init__(self, formatter):
+    logging.Handler.__init__(self)
+    self.setFormatter(formatter)
+    self.pid = None
+
+  def connect(self):
+    self.zctx = zmq.Context()
+    self.sock = self.zctx.socket(zmq.PUSH)
+    self.sock.connect("ipc:///tmp/logmessage")
+    self.pid = os.getpid()
+
+  def emit(self, record):
+    if os.getpid() != self.pid:
+      self.connect()
+
+    msg = self.format(record).rstrip('\n')
+    # print "SEND", repr(msg)
+    try:
+      self.sock.send(chr(record.levelno)+msg, zmq.NOBLOCK)
+    except zmq.error.Again:
+      # drop :/
+      pass
+
+cloudlog = log = SwagLogger()
+log.setLevel(logging.DEBUG)
+
+outhandler = logging.StreamHandler()
+log.addHandler(outhandler)
+
+log.addHandler(LogMessageHandler(SwagFormatter(log)))

selfdrive/thermal.py

@@ -0,0 +1,19 @@
+"""Methods for reading system thermal information."""
+import selfdrive.messaging as messaging
+
+def read_tz(x):
+  with open("/sys/devices/virtual/thermal/thermal_zone%d/temp" % x) as f:
+    ret = int(f.read())
+  return ret
+
+def read_thermal():
+  dat = messaging.new_message()
+  dat.init('thermal')
+  dat.thermal.cpu0 = read_tz(5)
+  dat.thermal.cpu1 = read_tz(7)
+  dat.thermal.cpu2 = read_tz(10)
+  dat.thermal.cpu3 = read_tz(12)
+  dat.thermal.mem = read_tz(2)
+  dat.thermal.gpu = read_tz(16)
+  dat.thermal.bat = read_tz(29)
+  return dat

selfdrive/ui/Makefile

@@ -0,0 +1,73 @@
+CC = clang
+CXX = clang++
+
+
+PHONELIBS = ../../phonelibs
+
+WARN_FLAGS = -Werror=implicit-function-declaration \
+             -Werror=incompatible-pointer-types \
+             -Werror=int-conversion \
+             -Werror=return-type \
+             -Werror=format-extra-args
+
+CFLAGS = -std=gnu11 -g -fPIC -O2 $(WARN_FLAGS)
+CXXFLAGS = -std=c++11 -g -fPIC -O2 $(WARN_FLAGS)
+
+ZMQ_FLAGS = -I$(PHONELIBS)/zmq/aarch64/include
+ZMQ_LIBS = -L$(PHONELIBS)/zmq/aarch64/lib \
+           -l:libczmq.a -l:libzmq.a \
+           -lgnustl_shared
+
+CEREAL_CFLAGS = -I$(PHONELIBS)/capnp-c/include
+CEREAL_LIBS = -L$(PHONELIBS)/capnp-c/aarch64/lib -l:libcapn.a
+CEREAL_OBJS = ../../cereal/gen/c/log.capnp.o
+
+NANOVG_FLAGS = -I$(PHONELIBS)/nanovg
+
+OPENGL_LIBS = -lGLESv3
+
+FRAMEBUFFER_LIBS = -lutils -lgui -lEGL
+
+OBJS = ui.o \
+       touch.o \
+       ../common/visionipc.o \
+       ../common/framebuffer.o \
+       $(PHONELIBS)/nanovg/nanovg.o \
+       $(CEREAL_OBJS)
+
+DEPS := $(OBJS:.o=.d)
+
+all: ui
+
+ui: $(OBJS)
+	@echo "[ LINK ] $@"
+	$(CXX) -fPIC -o '$@' $^ \
+	      $(FRAMEBUFFER_LIBS) \
+	      $(CEREAL_LIBS) \
+	      $(ZMQ_LIBS) \
+        -L/system/vendor/lib64 \
+        $(OPENGL_LIBS) \
+        -lcutils -lm -llog
+
+../common/framebuffer.o: ../common/framebuffer.cc
+	@echo "[ CXX ] $@"
+	$(CXX) $(CXXFLAGS) -MMD \
+           -I$(PHONELIBS)/android_frameworks_native/include \
+           -I$(PHONELIBS)/android_system_core/include \
+           -I$(PHONELIBS)/android_hardware_libhardware/include \
+           -c -o '$@' '$<'
+
+%.o: %.c
+	@echo "[ CC ] $@"
+	$(CC) $(CFLAGS) -MMD \
+          -I.. -I../.. \
+          $(NANOVG_FLAGS) \
+          $(ZMQ_FLAGS) \
+          $(CEREAL_CFLAGS) \
+          -c -o '$@' '$<'
+
+.PHONY: clean
+clean:
+	rm -f ui $(OBJS) $(DEPS)
+
+-include $(DEPS)

selfdrive/ui/touch.c

@@ -0,0 +1,63 @@
+#include <stdbool.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <assert.h>
+#include <sys/poll.h>
+#include <linux/input.h>
+
+#include "touch.h"
+
+void touch_init(TouchState *s) {
+  // synaptics touch screen on oneplus 3
+  s->fd = open("/dev/input/event4", O_RDONLY);
+  assert(s->fd >= 0);
+}
+
+int touch_poll(TouchState *s, int* out_x, int* out_y) {
+  assert(out_x && out_y);
+  bool up = false;
+  while (true) {
+    struct pollfd polls[] = {{
+      .fd = s->fd,
+      .events = POLLIN,
+    }};
+    int err = poll(polls, 1, 0);
+    if (err < 0) {
+      return -1;
+    }
+    if (!(polls[0].revents & POLLIN)) {
+      break;
+    }
+
+    struct input_event event;
+    err = read(polls[0].fd, &event, sizeof(event));
+    if (err < sizeof(event)) {
+      return -1;
+    }
+
+    switch (event.type) { 
+    case EV_ABS:
+      if (event.code == ABS_MT_POSITION_X) {
+        s->last_x = event.value;
+      } else if (event.code == ABS_MT_POSITION_Y) {
+        s->last_y = event.value;
+      }
+      break;
+    case EV_KEY:
+      if (event.code == BTN_TOOL_FINGER && event.value == 0) {
+        // finger up
+        up = true;
+      }
+      break;
+    default:
+      break;
+    }
+  }
+  if (up) {
+    // adjust for landscape
+    *out_x = 1920 - s->last_y;
+    *out_y = s->last_x;
+  }
+  return up;
+}
+

selfdrive/ui/touch.h

@@ -0,0 +1,12 @@
+#ifndef TOUCH_H
+#define TOUCH_H
+
+typedef struct TouchState {
+  int fd;
+  int last_x, last_y;
+} TouchState;
+
+void touch_init(TouchState *s);
+int touch_poll(TouchState *s, int *out_x, int *out_y);
+
+#endif

selfdrive/visiond/Makefile

@@ -0,0 +1,4 @@
+-include build_from_src.mk
+
+release:
+	@echo "visiond: this is a release"

selfdrive/visiond/README

@@ -0,0 +1,3 @@
+visiond runs the openpilot vision pipeline. Everything running between the camera hardware and model outputs and video logs lives here.
+
+Contact us if you'd like features added or support for your platform.