openpilot v0.2.1 release

Ford Fusion 2017 dbc can file. refs #1.

Dockerfile.openpilot

@@ -0,0 +1,19 @@
+FROM ubuntu:14.04
+ENV PYTHONUNBUFFERED 1
+
+RUN apt-get update && apt-get install -y build-essential screen wget bzip2 git libglib2.0-0
+
+RUN wget -nv -O /tmp/anaconda.sh https://repo.continuum.io/archive/Anaconda2-4.2.0-Linux-x86_64.sh && \
+  /bin/bash /tmp/anaconda.sh -b -p /opt/conda && \
+  rm /tmp/anaconda.sh
+
+ENV PATH /opt/conda/bin:$PATH
+
+RUN conda install numpy==1.11.2 && conda install scipy==0.18.1
+
+COPY requirements_openpilot.txt /tmp/
+RUN pip install -r /tmp/requirements_openpilot.txt
+
+ENV PYTHONPATH /tmp/openpilot:$PYTHONPATH
+
+COPY . /tmp/openpilot

LICENSE.openpilot

@@ -1,4 +1,4 @@
-Copyright (c) 2016, comma.ai
+Copyright (c) 2016, Comma.ai, Inc.
 
 Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
 

README.md

@@ -42,26 +42,42 @@ Directory structure
   - assets        -- Fonts for ui
   - boardd        -- Daemon to talk to the board
   - calibrationd  -- Camera calibration server
+  - car           -- Code that talks to the car and implements CarInterface
   - common        -- Shared C/C++ code for the daemons
   - controls      -- Python controls (PID loops etc) for the car
   - logcatd       -- Android logcat as a service
   - loggerd       -- Logger and uploader of car data
+  - radar         -- Code that talks to the radar and implements RadarInterface
   - sensord       -- IMU / GPS interface code
+  - test/plant    -- Car simulator running code through virtual maneuvers
   - ui            -- The UI
   - visiond       -- embedded vision pipeline
 
 To understand how the services interact, see `common/services.py`
 
+Testing on PC
+------
+
+There is rudimentary infrastructure to run a basic simulation and generate a report of openpilot's behavior in different scenarios.
+
+```bash
+# Requires working docker
+./run_docker_tests.sh
+```
+
+The results are written to `selfdrive/test/plant/out/index.html`
+
+More extensive testing infrastructure and simulation environments are coming soon.
+
 Adding Car Support
 ------
 
 It should be relatively easy to add support for the Honda CR-V Touring. The brake message is the same. Steering has a slightly different message with a different message id. Sniff CAN while using LKAS to find it.
 
-The Honda Accord uses different signalling for the steering and probably requires new hardware.
+The Honda Accord uses different signaling for the steering and probably requires new hardware.
 
 Adding other manufacturers besides Honda/Acura is doable but will be more of an undertaking.
 
-
 User Data / chffr Account / Crash Reporting
 ------
 
@@ -81,7 +97,7 @@ We welcome both pull requests and issues on
 [github](http://github.com/commaai/openpilot). See the TODO file for a list of
 good places to start.
 
-
+Want to get paid to work on openpilot? [comma.ai is hiring](http://comma.ai/hiring.html)
 
 Licensing
 ------

RELEASES.md

@@ -0,0 +1,25 @@
+Version 0.2.1  (2016-12-14)
+===========================
+  * Performance improvements, removal of more numpy
+  * Fix boardd process priority
+  * Make counter timer reset on use of steering wheel
+
+Version 0.2  (2016-12-12)
+=========================
+  * Car/Radar abstraction layers have shipped, see cereal/car.capnp
+  * controlsd has been refactored
+  * Shipped plant model and testing maneuvers
+  * visiond exits more gracefully now
+  * Hardware encoder in visiond should always init
+  * ui now turns off the screen after 30 seconds
+  * Switch to openpilot release branch for future releases
+  * Added preliminary Docker container to run tests on PC
+
+Version 0.1  (2016-11-29)
+=========================
+  * Initial release of openpilot
+  * Adaptive cruise control is working
+  * Lane keep assist is working
+  * Support for Acura ILX 2016 with AcuraWatch Plus
+  * Support for Honda Civic 2016 Touring Edition
+

TODO.md

@@ -1,14 +0,0 @@
-TODO
-======
-
-An incomplete list of known issues and desired featues.
-
-- TX and RX amounts on UI are wrong for a few frames at startup because we
-  subtract (total sent - 0). We should initialize sent bytes before displaying.
-
-- Rewrite common/dbc.py to be faster and cleaner, potentially in C++.
-
-- Add module and class level documentation where appropriate.
-
-- Fix lock file cleanup so there isn't always 1 pending upload when the vehicle
-  shuts off.

board/main.c

@@ -2,6 +2,7 @@
 //#define CAN_LOOPBACK_MODE
 //#define USE_INTERNAL_OSC
 //#define OLD_BOARD
+//#define ENABLE_CURRENT_SENSOR
 
 #define USB_VID 0xbbaa
 #define USB_PID 0xddcc
@@ -257,7 +258,11 @@ int get_health_pkt(void *dat) {
     uint8_t gas_interceptor_detected;
   } *health = dat;
   health->voltage = adc_get(ADCCHAN_VOLTAGE);
+#ifdef ENABLE_CURRENT_SENSOR
   health->current = adc_get(ADCCHAN_CURRENT);
+#else
+  health->current = 0;
+#endif
   health->started = (GPIOC->IDR & (1 << 13)) != 0;
   health->controls_allowed = controls_allowed;
   health->gas_interceptor_detected = gas_interceptor_detected;

cereal/__init__.py

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

cereal/car.capnp

@@ -0,0 +1,172 @@
+using Cxx = import "c++.capnp";
+$Cxx.namespace("cereal");
+
+@0x8e2af1e708af8b8d;
+
+# ******* main car state @ 100hz *******
+# all speeds in m/s
+
+struct CarState {
+  errors @0: List(Error);
+
+  # car speed
+  vEgo @1 :Float32;       # best estimate of speed
+  wheelSpeeds @2 :WheelSpeeds;
+
+  # gas pedal, 0.0-1.0
+  gas @3 :Float32;        # this is user + computer
+  gasPressed @4 :Bool;    # this is user pedal only
+
+  # brake pedal, 0.0-1.0
+  brake @5 :Float32;      # this is user pedal only
+  brakePressed @6 :Bool;  # this is user pedal only
+
+  # steering wheel
+  steeringAngle @7 :Float32;   # deg
+  steeringTorque @8 :Float32;  # TODO: standardize units
+  steeringPressed @9 :Bool;    # if the user is using the steering wheel
+
+  # cruise state
+  cruiseState @10 :CruiseState;
+
+  # button presses
+  buttonEvents @11 :List(ButtonEvent);
+
+  # which packets this state came from
+  canMonoTimes @12: List(UInt64);
+
+  struct WheelSpeeds {
+    # optional wheel speeds
+    fl @0 :Float32;
+    fr @1 :Float32;
+    rl @2 :Float32;
+    rr @3 :Float32;
+  }
+
+  struct CruiseState {
+    enabled @0: Bool;
+    speed @1: Float32;
+  }
+
+  enum Error {
+    # TODO: copy from error list
+    commIssue @0;
+    steerUnavailable @1;
+    brakeUnavailable @2;
+    gasUnavailable @3;
+    wrongGear @4;
+    doorOpen @5;
+    seatbeltNotLatched @6;
+    espDisabled @7;
+    wrongCarMode @8;
+    steerTemporarilyUnavailable @9;
+    reverseGear @10;
+    # ...
+  }
+
+  # send on change
+  struct ButtonEvent {
+    pressed @0: Bool;
+    type @1: Type;
+
+    enum Type {
+      unknown @0;
+      leftBlinker @1;
+      rightBlinker @2;
+      accelCruise @3;
+      decelCruise @4;
+      cancel @5;
+      altButton1 @6;
+      altButton2 @7;
+      altButton3 @8;
+    }
+  }
+}
+
+# ******* radar state @ 20hz *******
+
+struct RadarState {
+  errors @0: List(Error);
+  points @1: List(RadarPoint);
+
+  # which packets this state came from
+  canMonoTimes @2: List(UInt64);
+
+  enum Error {
+    notValid @0;
+  }
+
+  # similar to LiveTracks
+  # is one timestamp valid for all? I think so
+  struct RadarPoint {
+    trackId @0: UInt64;  # no trackId reuse
+
+    # these 3 are the minimum required
+    dRel @1: Float32; # m from the front bumper of the car
+    yRel @2: Float32; # m
+    vRel @3: Float32; # m/s
+
+    # these are optional and valid if they are not NaN
+    aRel @4: Float32; # m/s^2
+    yvRel @5: Float32; # m/s
+  }
+}
+
+# ******* car controls @ 100hz *******
+
+struct CarControl {
+  # must be true for any actuator commands to work
+  enabled @0: Bool;
+
+  # range from 0.0 - 1.0
+  gas @1: Float32;
+  brake @2: Float32;
+
+  # range from -1.0 - 1.0
+  steeringTorque @3 :Float32;
+
+  cruiseControl @4: CruiseControl;
+  hudControl @5: HUDControl;
+
+  struct CruiseControl {
+    cancel @0: Bool;
+    override @1: Bool;
+    speedOverride @2: Float32;
+    accelOverride @3: Float32;
+  }
+
+  struct HUDControl {
+    speedVisible @0: Bool;
+    setSpeed @1: Float32;
+    lanesVisible @2: Bool;
+    leadVisible @3: Bool;
+    visualAlert @4: VisualAlert;
+    audibleAlert @5: AudibleAlert;
+
+    enum VisualAlert {
+      # these are the choices from the Honda
+      # map as good as you can for your car
+      none @0;
+      fcw @1;
+      steerRequired @2;
+      brakePressed @3;
+      wrongGear @4;
+      seatbeltUnbuckled @5;
+      speedTooHigh @6;
+    }
+
+    enum AudibleAlert {
+      # these are the choices from the Honda
+      # map as good as you can for your car
+      none @0;
+      beepSingle @1;
+      beepTriple @2;
+      beepRepeated @3;
+      chimeSingle @4;
+      chimeDouble @5;
+      chimeRepeated @6;
+      chimeContinuous @7;
+    }
+  }
+}
+

cereal/log.capnp

@@ -85,11 +85,11 @@ struct Live20Data {
   ftMonoTime @7 :UInt64;
 
   # all deprecated
-  warpMatrix @0 :List(Float32);
-  angleOffset @1 :Float32;
-  calStatus @2 :Int8;
-  calCycle @8 :Int32;
-  calPerc @9 :Int8;
+  warpMatrixDEPRECATED @0 :List(Float32);
+  angleOffsetDEPRECATED @1 :Float32;
+  calStatusDEPRECATED @2 :Int8;
+  calCycleDEPRECATED @8 :Int32;
+  calPercDEPRECATED @9 :Int8;
 
   leadOne @3 :LeadData;
   leadTwo @4 :LeadData;
@@ -138,7 +138,7 @@ struct Live100Data {
   mdMonoTime @18 :UInt64;
 
   vEgo @0 :Float32;
-  aEgo @1 :Float32;
+  aEgoDEPRECATED @1 :Float32;
   vPid @2 :Float32;
   vTargetLead @3 :Float32;
   upAccelCmd @4 :Float32;
@@ -151,7 +151,7 @@ struct Live100Data {
   aTargetMax @11 :Float32;
   jerkFactor @12 :Float32;
   angleSteers @13 :Float32;
-  hudLead @14 :Int32;
+  hudLeadDEPRECATED @14 :Int32;
   cumLagMs @15 :Float32;
 
   enabled @19: Bool;

common/api/__init__.py

@@ -3,6 +3,6 @@ import requests
 def api_get(endpoint, method='GET', timeout=None, **params):
   backend = "https://api.commadotai.com/"
 
-  params['_version'] = "OPENPILOTv0.0"
+  params['_version'] = "OPENPILOTv0.2"
 
   return requests.request(method, backend+endpoint, timeout=timeout, params=params)

common/crash.py

@@ -5,6 +5,8 @@ import sys
 if os.getenv("NOLOG"):
   def capture_exception(*exc_info):
     pass
+  def bind_user(**kwargs):
+    pass
   def install():
     pass
 else:
@@ -16,6 +18,9 @@ else:
 
   capture_exception = client.captureException
 
+  def bind_user(**kwargs):
+    client.user_context(kwargs)
+
   def install():
     # installs a sys.excepthook
     __excepthook__ = sys.excepthook

common/dbc.py

@@ -1,6 +1,7 @@
 import re
-from collections import namedtuple
 import bitstring
+from binascii import hexlify
+from collections import namedtuple
 
 def int_or_float(s):
   # return number, trying to maintain int format
@@ -148,8 +149,8 @@ class dbc(object):
     if debug:
       print name
 
-    blen = (len(x[2])/2)*8
-    x2_int = int(x[2], 16)
+    blen = 8*len(x[2])
+    x2_int = int(hexlify(x[2]), 16)
 
     for s in msg[1]:
       if arr is not None and s[0] not in arr:

common/numpy_fast.py

@@ -1,2 +1,25 @@
 def clip(x, lo, hi):
   return max(lo, min(hi, x))
+
+
+def interp(x, xp, fp):
+  N = len(xp)
+  if not hasattr(x, '__iter__'):
+    hi = 0
+    while hi < N and x > xp[hi]:
+      hi += 1
+    low = hi - 1
+    return fp[-1] if hi == N and x > xp[low] else (
+      fp[0] if hi == 0 else
+      (x - xp[low]) * (fp[hi] - fp[low]) / (xp[hi] - xp[low]) + fp[low])
+
+  result = []
+  for v in x:
+    hi = 0
+    while hi < N and v > xp[hi]:
+      hi += 1
+    low = hi - 1
+    result.append(fp[-1] if hi == N and v > xp[low] else (fp[
+      0] if hi == 0 else (v - xp[low]) * (fp[hi] - fp[low]) / (xp[hi] - xp[low]
+                                                               ) + fp[low]))
+  return result

common/realtime.py

@@ -57,7 +57,7 @@ def set_realtime_priority(level):
     raise NotImplementedError
 
   tid = libc.syscall(NR_gettid)
-  subprocess.check_call(['chrt', '-f', '-p', str(level), str(tid)])
+  return subprocess.call(['chrt', '-f', '-p', str(level), str(tid)])
 
 
 class Ratekeeper(object):

common/services.py

@@ -39,14 +39,21 @@ service_list = {
 #   subscribes: health
 #   publishes:  thermal
 
+# **** processes that communicate with the outside world ****
+
 # boardd -- communicates with the car
 #   subscribes: sendcan
 #   publishes:  can, health
 
+# sensord -- publishes the IMU and GPS
+#   publishes:  sensorEvents, gpsNMEA
+
 # visiond -- talks to the cameras, runs the model, saves the videos
 #   subscribes: liveCalibration, sensorEvents
 #   publishes:  frame, encodeIdx, model, features
 
+# **** stateful data transformers ****
+
 # controlsd -- actually drives the car
 #   subscribes: can, thermal, model, live20
 #   publishes:  sendcan, live100
@@ -55,9 +62,6 @@ service_list = {
 #   subscribes: can, live100, model
 #   publishes:  live20, liveTracks
 
-# sensord -- publishes the IMU and GPS
-#   publishes:  sensorEvents, gpsNMEA
-
 # calibrationd -- places the camera box
 #   subscribes: features, live100
 #   publishes:  liveCalibration
@@ -80,3 +84,4 @@ service_list = {
 
 # logcatd -- fetches logcat info from android
 #   publishes:  androidLog
+

installation/files/continue.sh

@@ -7,16 +7,17 @@
 export GIT_SSH_COMMAND="ssh -i /data/data/com.termux/files/id_rsa_openpilot_ro"
 
 # check out the openpilot repo
-# TODO: release branch only
 if [ ! -d /data/openpilot ]; then
   cd /tmp
-  git clone git@github.com:commaai/openpilot.git
+  git clone git@github.com:commaai/openpilot.git -b release
   mv /tmp/openpilot /data/openpilot
 fi
 
-# update the openpilot repo
+# enter openpilot directory
 cd /data/openpilot
-git pull
+
+# removed automatic update from openpilot
+#git pull
 
 # start manager
 cd selfdrive

requirements_openpilot.txt

@@ -0,0 +1,12 @@
+Cython==0.24.1
+bitstring==3.1.5
+fastcluster==1.1.21
+h5py==2.6.0
+libusb1==1.5.0
+pycapnp==0.5.9
+pyzmq==15.4.0
+raven==5.23.0
+requests==2.10.0
+setproctitle==1.1.10
+simplejson==3.8.2
+-e git+https://github.com/commaai/le_python.git#egg=Logentries

run_docker_tests.sh

@@ -0,0 +1,7 @@
+#!/bin/bash
+set -e
+
+docker build -t tmppilot -f Dockerfile.openpilot .
+docker run --rm \
+  -v "$(pwd)"/selfdrive/test/plant/out:/tmp/openpilot/selfdrive/test/plant/out \
+  tmppilot /bin/sh -c 'cd /tmp/openpilot/selfdrive/test/plant && ./runtest.sh'

selfdrive/boardd/boardd.cc

@@ -4,6 +4,7 @@
 #include <string.h>
 #include <signal.h>
 #include <unistd.h>
+#include <sched.h>
 #include <sys/time.h>
 #include <sys/cdefs.h>
 #include <sys/types.h>
@@ -27,6 +28,8 @@ libusb_context *ctx = NULL;
 libusb_device_handle *dev_handle;
 pthread_mutex_t usb_lock;
 
+bool spoofing_started = false;
+
 // double the FIFO size
 #define RECV_SIZE (0x1000)
 #define TIMEOUT 0
@@ -147,7 +150,11 @@ void can_health(void *s) {
   // set fields
   healthData.setVoltage(health.voltage);
   healthData.setCurrent(health.current);
-  healthData.setStarted(health.started);
+  if (spoofing_started) {
+    healthData.setStarted(1);
+  } else {
+    healthData.setStarted(health.started);
+  }
   healthData.setControlsAllowed(health.controls_allowed);
   healthData.setGasInterceptorDetected(health.gas_interceptor_detected);
 
@@ -177,7 +184,7 @@ void can_send(void *s) {
   memset(send, 0, msg_count*0x10);
 
   for (int i = 0; i < msg_count; i++) {
-    auto cmsg = event.getCan()[i];
+    auto cmsg = event.getSendcan()[i];
     if (cmsg.getAddress() >= 0x800) {
       // extended
       send[i*4] = (cmsg.getAddress() << 3) | 5;
@@ -262,14 +269,27 @@ void *can_health_thread(void *crap) {
   return NULL;
 }
 
+int set_realtime_priority(int level) {
+  // should match python using chrt
+  struct sched_param sa;
+  memset(&sa, 0, sizeof(sa));
+  sa.sched_priority = level;
+  return sched_setscheduler(gettid(), SCHED_FIFO, &sa);
+}
+
 int main() {
   int err;
   printf("boardd: starting boardd\n");
 
   // set process priority
-  err = setpriority(PRIO_PROCESS, 0, -4);
+  err = set_realtime_priority(4);
   printf("boardd: setpriority returns %d\n", err);
 
+  // check the environment
+  if (getenv("STARTED")) {
+    spoofing_started = true;
+  }
+
   // connect to the board
   err = libusb_init(&ctx);
   assert(err == 0);

selfdrive/boardd/boardd.py

@@ -18,27 +18,25 @@ except Exception:
 # TODO: rewrite in C to save CPU
 
 # *** serialization functions ***
-def can_list_to_can_capnp(can_msgs):
+def can_list_to_can_capnp(can_msgs, msgtype='can'):
   dat = messaging.new_message()
-  dat.init('can', len(can_msgs))
+  dat.init(msgtype, 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]
+    if msgtype == 'sendcan':
+      cc = dat.sendcan[i]
+    else:
+      cc = dat.can[i]
+    cc.address = can_msg[0]
+    cc.busTime = can_msg[1]
+    cc.dat = can_msg[2]
+    cc.src = can_msg[3]
   return dat
 
-def can_capnp_to_can_list_old(dat, src_filter=[]):
+def can_capnp_to_can_list(can, src_filter=None):
   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])
+  for msg in can:
+    if src_filter is None or msg.src in src_filter:
+      ret.append((msg.address, msg.busTime, msg.dat, msg.src))
   return ret
 
 # *** can driver ***

selfdrive/car/fingerprints.py

@@ -1,8 +1,12 @@
 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
+    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,
+    # sent messages
+    0xe4: 5, 0x1fa: 8, 0x200: 3, 0x30c: 8, 0x33d: 4,
   },
   "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
+    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,
+    # sent messages
+    0xe4: 5, 0x1fa: 8, 0x200: 3, 0x30c: 8, 0x33d: 5, 0x35e: 8, 0x39f: 8,
   }
 }

selfdrive/car/honda/can_parser.py

@@ -2,7 +2,7 @@ import os
 import dbcs
 from collections import defaultdict
 
-from selfdrive.controls.lib.hondacan import fix
+from selfdrive.car.honda.hondacan import fix
 from common.realtime import sec_since_boot
 from common.dbc import dbc
 
@@ -59,21 +59,21 @@ class CANParser(object):
     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:
+    for msg, ts, cdat, _ in can_recv:
       idxs = self._message_indices[msg]
       if idxs:
-        self.msgs_upd += [msg]
+        self.msgs_upd.append(msg)
         # read the entire message
-        out = self.can_dbc.decode([msg, 0, cdat])[1]
+        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')
+          ck_portion = cdat[:-1] + chr(ord(cdat[-1]) & 0xF0)
           # recalculate checksum
           msg_vl = fix(ck_portion, msg)
           # compare recalculated vs received checksum
-          if msg_vl != cdat.decode('hex'):
+          if msg_vl != cdat:
             print hex(msg), "CHECKSUM FAIL"
             self.ck[msg] = False
             self.ok[msg] = False
@@ -112,7 +112,7 @@ class CANParser(object):
     # 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!"
+      #print "CAN INVALID!"
       self.can_valid = False
 
   def _check_dead_msgs(self):

selfdrive/car/honda/carcontroller.py

@@ -1,25 +1,54 @@
 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
 
+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]
+
+
+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
+
+import selfdrive.car.honda.hondacan as hondacan
+
 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.
 
+    # redundant safety check with the board
+    self.controls_allowed = False
+
   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, \
@@ -61,9 +90,9 @@ class CarController(object):
     #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,
+    hud = HUDData(int(pcm_accel), int(hud_v_cruise), 0x41, hud_car,
                   0xc1, 0x41, hud_lanes + steer_required,
-                  snd_beep, 0x48, (snd_chime << 5) + fcw_display, acc_alert)
+                  int(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
@@ -71,14 +100,11 @@ class CarController(object):
 
     # **** 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
+    STEER_MAX = 0xF00
     GAS_OFFSET = 328
 
     # steer torque is converted back to CAN reference (positive when steering right)
@@ -87,51 +113,37 @@ class CarController(object):
     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
+    if apply_gas > 0 and (apply_brake != 0 or CS.brake_pressed):
+      print "CANCELLING GAS", apply_brake
       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 
+    # no computer brake if the gas is being pressed
     if CS.car_gas > 0 and apply_brake != 0:
+      print "CANCELLING BRAKE"
       apply_brake = 0
 
+    # any other cp.vl[0x18F]['STEER_STATUS'] is common and can happen during user override. sending 0 torque to avoid EPS sending error 5
+    if CS.steer_not_allowed:
+      print "STEER ALERT, TORQUE INHIBITED"
+      apply_steer = 0
+
+    # *** entry into controls state ***
     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
+    # *** exit from controls state on cancel, gas, or brake ***
     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:
+        CS.user_gas_pressed or (CS.pedal_gas > 0 and CS.brake_only)) and self.controls_allowed:
+      print "CONTROLS ARE DEAD"
       self.controls_allowed = False
 
-    # 5 is a permanent fault, no torque request will be fullfilled
+    # *** controls fail on steer error, brake error, or invalid can ***
     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
@@ -140,13 +152,6 @@ class CarController(object):
       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 = []
 
@@ -160,7 +165,6 @@ class CarController(object):
       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
@@ -182,4 +186,5 @@ class CarController(object):
       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())
+    sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan').to_bytes())
+

selfdrive/car/honda/carstate.py

@@ -1,12 +1,12 @@
 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.boardd.boardd import can_capnp_to_can_list
 from selfdrive.config import VehicleParams
 from common.realtime import sec_since_boot
 
+from selfdrive.car.fingerprints import fingerprints
+from selfdrive.car.honda.can_parser import CANParser
 
 def get_can_parser(civic, brake_only):
   # this function generates lists for signal, messages and initial values
@@ -46,6 +46,7 @@ def get_can_parser(civic, brake_only):
       ("LEFT_BLINKER", 0x326, 0),
       ("RIGHT_BLINKER", 0x326, 0),
       ("COUNTER", 0x324, 0),
+      ("ENGINE_RPM", 0x17C, 0)
     ]
     checks = [
       (0x14a, 100),
@@ -97,6 +98,7 @@ def get_can_parser(civic, brake_only):
       ("LEFT_BLINKER", 0x294, 0),
       ("RIGHT_BLINKER", 0x294, 0),
       ("COUNTER", 0x324, 0),
+      ("ENGINE_RPM", 0x17C, 0)
     ]
     checks = [
       (0x156, 100),
@@ -130,7 +132,7 @@ def fingerprint(logcan):
     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):
+      for adr, _, msg, idx in can_capnp_to_can_list(a.can):
         # pedal
         if adr == 0x201 and idx == 0:
           brake_only = False
@@ -153,6 +155,7 @@ def fingerprint(logcan):
     if len(possible_cars) == 1 and st is not None and (sec_since_boot()-st) > 0.1:
       break
     elif len(possible_cars) == 0:
+      print finger
       raise Exception("car doesn't match any fingerprints")
 
   print "fingerprinted", possible_cars[0]
@@ -183,6 +186,9 @@ class CarState(object):
     self.cruise_setting = 0
     self.blinker_on = 0
 
+    self.left_blinker_on = 0
+    self.right_blinker_on = 0
+
     # TODO: actually make this work
     self.a_ego = 0.
 
@@ -190,16 +196,9 @@ class CarState(object):
     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]))
+    self.VP = VehicleParams(self.civic, self.brake_only, self.torque_mod)
 
+  def update(self, can_pub_main):
     cp = self.cp
     cp.update_can(can_pub_main)
 
@@ -215,6 +214,11 @@ class CarState(object):
     self.prev_cruise_setting = self.cruise_setting
     self.prev_blinker_on = self.blinker_on
 
+    self.prev_left_blinker_on = self.left_blinker_on
+    self.prev_right_blinker_on = self.right_blinker_on
+
+    self.rpm = cp.vl[0x17C]['ENGINE_RPM']
+
     # ******************* 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']])
@@ -252,6 +256,8 @@ class CarState(object):
       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']
+      self.left_blinker_on = cp.vl[0x326]['LEFT_BLINKER']
+      self.right_blinker_on = cp.vl[0x326]['RIGHT_BLINKER']
     else:
       self.gear_shifter = cp.vl[0x1A3]['GEAR_SHIFTER']
       self.angle_steers = cp.vl[0x156]['STEER_ANGLE']
@@ -261,6 +267,8 @@ class CarState(object):
       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.left_blinker_on = cp.vl[0x294]['LEFT_BLINKER']
+      self.right_blinker_on = 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']
@@ -272,4 +280,3 @@ class CarState(object):
     self.hud_lead = cp.vl[0x30C]['HUD_LEAD']
     self.counter_pcm = cp.vl[0x324]['COUNTER']
 
-    return canMonoTimes

selfdrive/car/honda/interface.py

@@ -0,0 +1,238 @@
+#!/usr/bin/env python
+import time
+import numpy as np
+
+from selfdrive.config import Conversions as CV
+from selfdrive.car.honda.carstate import CarState
+from selfdrive.car.honda.carcontroller import CarController, AH
+from selfdrive.boardd.boardd import can_capnp_to_can_list
+
+from cereal import car
+
+import zmq
+from common.services import service_list
+import selfdrive.messaging as messaging
+
+# Car button codes
+class CruiseButtons:
+  RES_ACCEL   = 4
+  DECEL_SET   = 3
+  CANCEL      = 2
+  MAIN        = 1
+
+#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
+
+
+class CarInterface(object):
+  def __init__(self, read_only=False):
+    context = zmq.Context()
+    self.logcan = messaging.sub_sock(context, service_list['can'].port)
+
+    self.frame = 0
+    self.can_invalid_count = 0
+
+    # *** init the major players ***
+    self.CS = CarState(self.logcan)
+
+    # sending if read only is False
+    if not read_only:
+      self.sendcan = messaging.pub_sock(context, service_list['sendcan'].port)
+      self.CC = CarController()
+
+  def getVehicleParams(self):
+    return self.CS.VP
+
+  # returns a car.CarState
+  def update(self):
+    # ******************* do can recv *******************
+    can_pub_main = []
+    canMonoTimes = []
+    for a in messaging.drain_sock(self.logcan):
+      canMonoTimes.append(a.logMonoTime)
+      can_pub_main.extend(can_capnp_to_can_list(a.can, [0,2]))
+    self.CS.update(can_pub_main)
+
+    # create message
+    ret = car.CarState.new_message()
+
+    # speeds
+    ret.vEgo = self.CS.v_ego
+    ret.wheelSpeeds.fl = self.CS.cp.vl[0x1D0]['WHEEL_SPEED_FL']
+    ret.wheelSpeeds.fr = self.CS.cp.vl[0x1D0]['WHEEL_SPEED_FR']
+    ret.wheelSpeeds.rl = self.CS.cp.vl[0x1D0]['WHEEL_SPEED_RL']
+    ret.wheelSpeeds.rr = self.CS.cp.vl[0x1D0]['WHEEL_SPEED_RR']
+
+    # gas pedal
+    ret.gas = self.CS.car_gas / 256.0
+    if self.CS.VP.brake_only:
+      ret.gasPressed = self.CS.pedal_gas > 0
+    else:
+      ret.gasPressed = self.CS.user_gas_pressed
+
+    # brake pedal
+    ret.brake = self.CS.user_brake
+    ret.brakePressed = self.CS.brake_pressed != 0
+
+    # steering wheel
+    # TODO: units
+    ret.steeringAngle = self.CS.angle_steers
+    ret.steeringTorque = self.CS.cp.vl[0x18F]['STEER_TORQUE_SENSOR']
+    ret.steeringPressed = self.CS.steer_override
+
+    # cruise state
+    ret.cruiseState.enabled = self.CS.pcm_acc_status != 0
+    ret.cruiseState.speed = self.CS.v_cruise_pcm * CV.KPH_TO_MS
+
+    # TODO: button presses
+    buttonEvents = []
+
+    if self.CS.left_blinker_on != self.CS.prev_left_blinker_on:
+      be = car.CarState.ButtonEvent.new_message()
+      be.type = 'leftBlinker'
+      be.pressed = self.CS.left_blinker_on != 0
+      buttonEvents.append(be)
+
+    if self.CS.right_blinker_on != self.CS.prev_right_blinker_on:
+      be = car.CarState.ButtonEvent.new_message()
+      be.type = 'rightBlinker'
+      be.pressed = self.CS.right_blinker_on != 0
+      buttonEvents.append(be)
+
+    if self.CS.cruise_buttons != self.CS.prev_cruise_buttons:
+      be = car.CarState.ButtonEvent.new_message()
+      be.type = 'unknown'
+      if self.CS.cruise_buttons != 0:
+        be.pressed = True
+        but = self.CS.cruise_buttons
+      else:
+        be.pressed = False
+        but = self.CS.prev_cruise_buttons
+      if but == CruiseButtons.RES_ACCEL:
+        be.type = 'accelCruise'
+      elif but == CruiseButtons.DECEL_SET:
+        be.type = 'decelCruise'
+      elif but == CruiseButtons.CANCEL:
+        be.type = 'cancel'
+      elif but == CruiseButtons.MAIN:
+        be.type = 'altButton3'
+      buttonEvents.append(be)
+
+    if self.CS.cruise_setting != self.CS.prev_cruise_setting:
+      be = car.CarState.ButtonEvent.new_message()
+      be.type = 'unknown'
+      if self.CS.cruise_setting != 0:
+        be.pressed = True
+        but = self.CS.cruise_setting
+      else:
+        be.pressed = False
+        but = self.CS.prev_cruise_setting
+      if but == 1:
+        be.type = 'altButton1'
+      # TODO: more buttons?
+      buttonEvents.append(be)
+    ret.buttonEvents = buttonEvents
+
+    # errors
+    # TODO: I don't like the way capnp does enums
+    # These strings aren't checked at compile time
+    errors = []
+    if not self.CS.can_valid:
+      self.can_invalid_count += 1
+      if self.can_invalid_count >= 5:
+        errors.append('commIssue')
+    else:
+      self.can_invalid_count = 0
+    if self.CS.steer_error:
+      errors.append('steerUnavailable')
+    elif self.CS.steer_not_allowed:
+      errors.append('steerTemporarilyUnavailable')
+    if self.CS.brake_error:
+      errors.append('brakeUnavailable')
+    if not self.CS.gear_shifter_valid:
+      errors.append('wrongGear')
+    if not self.CS.door_all_closed:
+      errors.append('doorOpen')
+    if not self.CS.seatbelt:
+      errors.append('seatbeltNotLatched')
+    if self.CS.esp_disabled:
+      errors.append('espDisabled')
+    if not self.CS.main_on:
+      errors.append('wrongCarMode')
+    if self.CS.gear_shifter == 2:
+      errors.append('reverseGear')
+
+    ret.errors = errors
+    ret.canMonoTimes = canMonoTimes
+
+    # cast to reader so it can't be modified
+    #print ret
+    return ret.as_reader()
+
+  # pass in a car.CarControl
+  # to be called @ 100hz
+  def apply(self, c):
+    #print c
+
+    if c.hudControl.speedVisible:
+      hud_v_cruise = c.hudControl.setSpeed * CV.MS_TO_KPH
+    else:
+      hud_v_cruise = 255
+
+    hud_alert = {
+      "none": AH.NONE, 
+      "fcw": AH.FCW,
+      "steerRequired": AH.STEER,
+      "brakePressed": AH.BRAKE_PRESSED,
+      "wrongGear": AH.GEAR_NOT_D,
+      "seatbeltUnbuckled": AH.SEATBELT,
+      "speedTooHigh": AH.SPEED_TOO_HIGH}[str(c.hudControl.visualAlert)]
+
+    snd_beep, snd_chime = {
+      "none": (BP.MUTE, CM.MUTE),
+      "beepSingle": (BP.SINGLE, CM.MUTE),
+      "beepTriple": (BP.TRIPLE, CM.MUTE),
+      "beepRepeated": (BP.REPEATED, CM.MUTE),
+      "chimeSingle": (BP.MUTE, CM.SINGLE),
+      "chimeDouble": (BP.MUTE, CM.DOUBLE),
+      "chimeRepeated": (BP.MUTE, CM.REPEATED),
+      "chimeContinuous": (BP.MUTE, CM.CONTINUOUS)}[str(c.hudControl.audibleAlert)]
+
+    pcm_accel = int(np.clip(c.cruiseControl.accelOverride/1.4,0,1)*0xc6)
+
+    self.CC.update(self.sendcan, c.enabled, self.CS, self.frame, \
+      c.gas, c.brake, c.steeringTorque, \
+      c.cruiseControl.speedOverride, \
+      c.cruiseControl.override, \
+      c.cruiseControl.cancel, \
+      pcm_accel, \
+      hud_v_cruise, c.hudControl.lanesVisible, \
+      hud_show_car = c.hudControl.leadVisible, \
+      hud_alert = hud_alert, \
+      snd_beep = snd_beep, \
+      snd_chime = snd_chime)
+
+    self.frame += 1
+    return not (c.enabled and not self.CC.controls_allowed)
+
+
+if __name__ == "__main__":
+  CI = CarInterface(read_only=True)
+  while 1:
+    cs = CI.update()
+    print(chr(27) + "[2J")
+    print cs
+    time.sleep(0.1)
+

selfdrive/config.py

@@ -57,12 +57,18 @@ class UIParams:
   car_color = 110
 
 class VehicleParams:
-  def __init__(self, civic):
+  def __init__(self, civic, brake_only=False, torque_mod=False):
     if civic:
       self.wheelbase = 2.67
       self.steer_ratio = 15.3
       self.slip_factor = 0.0014
+      self.civic = True
     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
+      self.civic = False
+    self.brake_only = brake_only
+    self.torque_mod = torque_mod
+    self.ui_speed_fudge = 1.01 if self.civic else 1.025
+

selfdrive/controls/controlsd.py

@@ -1,101 +1,137 @@
 #!/usr/bin/env python
+import os
 import zmq
 import numpy as np
+import selfdrive.messaging as messaging
 
+from cereal import car
+
+from common.numpy_fast import clip
+
+from selfdrive.config import Conversions as CV
 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
 
+from selfdrive.controls.lib.alertmanager import AlertManager
+
+car_type = os.getenv("CAR")
+if car_type is not None:
+  exec('from selfdrive.car.'+car_type+'.interface import CarInterface')
+else:
+  from selfdrive.car.honda.interface import CarInterface
+
+V_CRUISE_MAX = 144
+V_CRUISE_MIN = 8
+V_CRUISE_DELTA = 8
+V_CRUISE_ENABLE_MIN = 40
+
 def controlsd_thread(gctx, rate=100):  #rate in Hz
   # *** log ***
   context = zmq.Context()
   live100 = messaging.pub_sock(context, service_list['live100'].port)
+
   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)
+  health = messaging.sub_sock(context, service_list['health'].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()
+  # connects to can and sendcan
+  CI = CarInterface()
+  VP = CI.getVehicleParams()
 
   PP = PathPlanner(model)
   AC = AdaptiveCruise(live20)
 
+  AM = AlertManager()
+
   LoC = LongControl()
   LaC = LatControl()
 
-  # *** control initial values ***
-  apply_brake = 0
+  # controls enabled state
   enabled = False
+  last_enable_request = 0
 
-  # *** 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
-
+  # learned angle offset
   angle_offset = 0
 
-  max_enable_speed = 57.   # ~91 mph
+  # rear view camera state
+  rear_view_toggle = False
 
-  pcm_threshold = 25.*CV.MPH_TO_MS    # below this speed pcm cancels
-
-  overtemp = True
+  v_cruise_kph = 255
 
   # 0.0 - 1.0
   awareness_status = 0.0
 
+  soft_disable_timer = None
+
   # start the loop
   set_realtime_priority(2)
 
-  rk = Ratekeeper(rate)
+  rk = Ratekeeper(rate, print_delay_threshold=2./1000)
   while 1:
     cur_time = sec_since_boot()
 
     # read CAN
-    canMonoTimes = CS.update(logcan)
+    CS = CI.update()
 
-    # **** rearview mirror management ***
-    if CS.cruise_setting == 1 and CS.prev_cruise_setting == 0:
-      rear_view_toggle = not rear_view_toggle
+    # did it request to enable?
+    enable_request, enable_condition = False, False
 
-    # 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
+    if enabled:
+      # gives the user 6 minutes
+      awareness_status -= 1.0/(100*60*6)
+      if awareness_status <= 0.:
+        AM.add("driverDistracted", enabled)
+
+    # reset awareness status on steering
+    if CS.steeringPressed:
+      awareness_status = 1.0
+
+    # handle button presses
+    for b in CS.buttonEvents:
+      print b
+
+      # reset awareness on any user action
+      awareness_status = 1.0
+
+      # button presses for rear view
+      if b.type == "leftBlinker" or b.type == "rightBlinker":
+        if b.pressed:
+          rear_view_toggle = True
+        else:
+          rear_view_toggle = False
+
+      if b.type == "altButton1" and b.pressed:
+        rear_view_toggle = not rear_view_toggle
+
+      if not VP.brake_only and enabled and not b.pressed:
+        if b.type == "accelCruise":
+          v_cruise_kph = v_cruise_kph - (v_cruise_kph % V_CRUISE_DELTA) + V_CRUISE_DELTA
+        elif b.type == "decelCruise":
+          v_cruise_kph = v_cruise_kph - (v_cruise_kph % V_CRUISE_DELTA) - V_CRUISE_DELTA
+        v_cruise_kph = clip(v_cruise_kph, V_CRUISE_MIN, V_CRUISE_MAX)
+
+      if not enabled and b.type in ["accelCruise", "decelCruise"] and not b.pressed:
+        enable_request = True 
+
+      # do disable on button down
+      if b.type == "cancel" and b.pressed:
+        AM.add("disable", enabled)
+
+    # *** health checking logic ***
+    hh = messaging.recv_sock(health)
+    if hh is not None:
+      # if the board isn't allowing controls but somehow we are enabled!
+      if not hh.health.controlsAllowed and enabled:
+        AM.add("controlsMismatch", enabled)
 
     # *** thermal checking logic ***
 
@@ -105,130 +141,57 @@ def controlsd_thread(gctx, rate=100):  #rate in Hz
       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)
+      if any(t > 950 for t in cpu_temps):
+        AM.add("overheat", enabled)
 
     # *** getting model logic ***
-    PP.update(cur_time, CS.v_ego)
+    PP.update(cur_time, CS.vEgo)
 
     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)
+      angle_offset = learn_angle_offset(enabled, CS.vEgo, angle_offset, np.asarray(PP.d_poly), LaC.y_des, CS.steeringPressed)
 
-    # 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
+    # disable if the pedals are pressed while engaged, this is a user disable
     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
+      if CS.gasPressed or CS.brakePressed:
+        AM.add("disable", enabled)
 
-    # 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
+    if enable_request:
+      # check for pressed pedals
+      if CS.gasPressed or CS.brakePressed:
+        AM.add("pedalPressed", enabled)
+        enable_request = False
+      else:
+        print "enabled pressed at", cur_time
+        last_enable_request = cur_time
 
-    # 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]
+    if VP.brake_only:
+      enable_condition = ((cur_time - last_enable_request) < 0.2) and CS.cruiseState.enabled
+    else:
+      enable_condition = enable_request
 
-    # enabling
-    if enable_condition and not enabled and len(alert_id) == 0:
+    if enable_request or enable_condition or enabled:
+      # add all alerts from car
+      for alert in CS.errors:
+        AM.add(alert, enabled)
+
+      if AC.dead:
+        AM.add("radarCommIssue", enabled)
+
+      if PP.dead:
+        AM.add("modelCommIssue", enabled)
+
+    if enable_condition and not enabled and not AM.alertPresent():
       print "*** enabling controls"
 
-      #enable both lateral and longitudinal controls
+      # beep for enabling
+      AM.add("enable", enabled)
+
+      # 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)))
+      v_cruise_kph = int(round(max(CS.vEgo * CV.MS_TO_KPH * VP.ui_speed_fudge, V_CRUISE_ENABLE_MIN)))
 
       # 6 minutes driver you're on
       awareness_status = 1.0
@@ -236,67 +199,69 @@ def controlsd_thread(gctx, rate=100):  #rate in Hz
       # reset the PID loops
       LaC.reset()
       # start long control at actual speed
-      LoC.reset(v_pid = CS.v_ego)
+      LoC.reset(v_pid = CS.vEgo)
 
-      alert_id += [AI.ENABLE]
+    if VP.brake_only and CS.cruiseState.enabled:
+      v_cruise_kph = CS.cruiseState.speed * CV.MS_TO_KPH
 
-    if v_cruise != 255 and not CS.brake_only:
-      v_cruise = np.clip(v_cruise, v_cruise_min, v_cruise_max)
+    # *** put the adaptive in adaptive cruise control ***
+    AC.update(cur_time, CS.vEgo, CS.steeringAngle, LoC.v_pid, awareness_status, VP)
 
-    # **** 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?
+    final_gas, final_brake = LoC.update(enabled, CS.vEgo, v_cruise_kph, AC.v_target_lead, AC.a_target, AC.jerk_factor, VP)
 
     # *** 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
+    final_steer, sat_flag = LaC.update(enabled, CS.vEgo, CS.steeringAngle, CS.steeringPressed, PP.d_poly, angle_offset, VP)
 
     # ***** handle alerts ****
     # send a "steering required alert" if saturation count has reached the limit
     if sat_flag:
-      alert_id += [AI.STEER_SATURATED]
+      AM.add("steerSaturated", enabled)
 
-    # 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)
+    if enabled and AM.alertShouldDisable():
+      print "DISABLING IMMEDIATELY ON ALERT"
+      enabled = False
 
-    # 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
+    if enabled and AM.alertShouldSoftDisable():
+      if soft_disable_timer is None:
+        soft_disable_timer = 3 * rate
+      elif soft_disable_timer == 0:
+        print "SOFT DISABLING ON ALERT"
+        enabled = False
+      else:
+        soft_disable_timer -= 1
     else:
-      hud_v_cruise = v_cruise
+      soft_disable_timer = None
+      
 
-    # *** 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)
+    # *** push the alerts to current ***
+    alert_text_1, alert_text_2, visual_alert, audible_alert = AM.process_alerts(cur_time)
+
+    # ***** control the car *****
+    CC = car.CarControl.new_message()
+
+    CC.enabled = enabled
+
+    CC.gas = float(final_gas)
+    CC.brake = float(final_brake)
+    CC.steeringTorque = float(final_steer)
+
+    CC.cruiseControl.override = True
+    CC.cruiseControl.cancel = bool((not VP.brake_only) or (not enabled and CS.cruiseState.enabled))    # always cancel if we have an interceptor
+    CC.cruiseControl.speedOverride = float((LoC.v_pid - .3) if (VP.brake_only and final_brake == 0.) else 0.0)
+    CC.cruiseControl.accelOverride = float(AC.a_pcm)
+
+    CC.hudControl.setSpeed = float(v_cruise_kph * CV.KPH_TO_MS)
+    CC.hudControl.speedVisible = enabled
+    CC.hudControl.lanesVisible = enabled
+    CC.hudControl.leadVisible = bool(AC.has_lead)
+
+    CC.hudControl.visualAlert = visual_alert
+    CC.hudControl.audibleAlert = audible_alert
+
+    # this alert will apply next controls cycle
+    if not CI.apply(CC):
+      AM.add("controlsFailed", enabled)
 
     # ***** publish state to logger *****
 
@@ -304,14 +269,14 @@ def controlsd_thread(gctx, rate=100):  #rate in Hz
     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]
+    # show rear view camera on phone if in reverse gear or when button is pressed
+    dat.live100.rearViewCam = ('reverseGear' in CS.errors) or rear_view_toggle
+    dat.live100.alertText1 = alert_text_1
+    dat.live100.alertText2 = alert_text_2
     dat.live100.awarenessStatus = max(awareness_status, 0.0) if enabled else 0.0
 
     # what packets were used to process
-    dat.live100.canMonoTimes = canMonoTimes
+    dat.live100.canMonoTimes = list(CS.canMonoTimes)
     dat.live100.mdMonoTime = PP.logMonoTime
     dat.live100.l20MonoTime = AC.logMonoTime
 
@@ -319,15 +284,13 @@ def controlsd_thread(gctx, rate=100):  #rate in Hz
     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
+    dat.live100.vEgo = CS.vEgo
+    dat.live100.angleSteers = CS.steeringAngle
+    dat.live100.steerOverride = CS.steeringPressed
 
     # longitudinal control state
     dat.live100.vPid = float(LoC.v_pid)
-    dat.live100.vCruise = float(v_cruise)
+    dat.live100.vCruise = float(v_cruise_kph)
     dat.live100.upAccelCmd = float(LoC.Up_accel_cmd)
     dat.live100.uiAccelCmd = float(LoC.Ui_accel_cmd)
 
@@ -356,3 +319,4 @@ def main(gctx=None):
 
 if __name__ == "__main__":
   main()
+

selfdrive/controls/lib/adaptivecruise.py

@@ -1,41 +1,41 @@
-import selfdrive.messaging as messaging
+import math
 import numpy as np
+from common.numpy_fast import clip, interp
+import selfdrive.messaging as messaging
 
 # 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.])
+_A_CRUISE_MIN_V  = [-1.0, -.8, -.67, -.5, -.30]
+_A_CRUISE_MIN_BP = [   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.])
+_A_CRUISE_MAX_V  = [1., 1., .8, .5, .30]
+_A_CRUISE_MAX_BP = [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_cruise_min = interp(v_ego, _A_CRUISE_MIN_BP, _A_CRUISE_MIN_V)
+  a_cruise_max = interp(v_ego, _A_CRUISE_MAX_BP, _A_CRUISE_MAX_V)
+  return np.vstack([a_cruise_min, a_cruise_max])
 
-  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.])
+_A_TOTAL_MAX_V = [1.5, 1.9, 3.2]
+_A_TOTAL_MAX_BP = [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_total_max = 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_x_allowed = math.sqrt(max(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)
+  a_target[1] = min(a_target[1], a_x_allowed)
+  a_pcm = min(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)
+  a_lead = min(a_lead + a_lead_threshold, 0)
   return a_lead
 
 def calc_desired_distance(v_lead):
@@ -46,12 +46,12 @@ def calc_desired_distance(v_lead):
 
 
 #linear slope
-_L_SLOPE_V = np.asarray([0.40, 0.10])
-_L_SLOPE_BP = np.asarray([0.,  40])
+_L_SLOPE_V = [0.40, 0.10]
+_L_SLOPE_BP = [0.,  40]
 
 # parabola slope
-_P_SLOPE_V = np.asarray([1.0, 0.25])
-_P_SLOPE_BP = np.asarray([0., 40])
+_P_SLOPE_V = [1.0, 0.25]
+_P_SLOPE_BP = [0., 40]
 
 def calc_desired_speed(d_lead, d_des, v_lead, a_lead):
   #*** compute desired speed ***
@@ -64,8 +64,8 @@ def calc_desired_speed(d_lead, d_des, v_lead, a_lead):
   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)
+  l_slope = interp(v_lead, _L_SLOPE_BP, _L_SLOPE_V)
+  p_slope = 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
@@ -79,41 +79,41 @@ def calc_desired_speed(d_lead, d_des, v_lead, a_lead):
     # 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)
+    v_rel_des = min(v_rel_des_1, v_rel_des_2)
+    v_rel_des = max(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)
+    v_rel_des = max(v_rel_des, max_runaway_speed)
   else:
-    v_rel_des = np.sqrt(2 * (d_lead - d_des - x_parabola_offset) * p_slope)
+    v_rel_des = math.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_shift = max(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)
+  v_coast = min(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)
+  a_critical =  - max(0., v_rel + v_offset)**2/max(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])
+_A_CORR_BY_SPEED_V = [0.4, 0.4, 0]
 # speeds
-_A_CORR_BY_SPEED_BP = np.asarray([0., 5., 20.])
+_A_CORR_BY_SPEED_BP = [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):
+  if v_ref > min(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.
@@ -123,31 +123,31 @@ def calc_positive_accel_limit(d_lead, d_des, v_ego, v_rel, v_ref, v_rel_ref, v_c
       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)
+      a_max = max(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)
+    a_max = a_max + interp(v_ego, _A_CORR_BY_SPEED_BP, _A_CORR_BY_SPEED_V) \
+                  * clip(-v_rel / 4., -.5, 1)
   return a_max
 
 # arbitrary limits to avoid too high accel being computed
-_A_SAT = np.asarray([-10., 5.])
+_A_SAT = [-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.])
+_A_LEAD_LOW_SPEED_V = [0., 1.]
 
 # speed break points
-_A_LEAD_LOW_SPEED_BP = np.asarray([0., 10.])
+_A_LEAD_LOW_SPEED_BP = [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])
+_DECEL_OFFSET_V = [-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.])
+_DECEL_OFFSET_BP = [0., 4., 15., 30, 40.]
 
 
 def calc_acc_accel_limits(d_lead, d_des, v_ego, v_pid, v_lead, v_rel, a_lead,
@@ -159,8 +159,8 @@ def calc_acc_accel_limits(d_lead, d_des, v_ego, v_pid, v_lead, v_rel, a_lead,
   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
+  a_lead_contr = a_lead * 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,
@@ -178,15 +178,15 @@ def calc_acc_accel_limits(d_lead, d_des, v_ego, v_pid, v_lead, v_rel, a_lead,
     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)
+    decel_offset = 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])
+    a_target[0] = min(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])
+  a_target[0] = min(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()
@@ -208,8 +208,8 @@ def calc_jerk_factor(d_lead, v_rel):
   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)
+    jerk_factor = max(a_critical - a_offset, 0.)/5.
+    jerk_factor = min(jerk_factor, jerk_factor_max)
   return jerk_factor
 
 
@@ -223,27 +223,27 @@ def calc_ttc(d_rel, v_rel, a_rel, v_lead):
   # 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)
+  a_rel = min(a_rel, v_lead/t_decel)
 
   delta = v_rel**2 + 2 * d_rel * a_rel
   # assign an arbitrary high ttc value if there is no solution to ttc
   if delta < 0.1:
     ttc = 5.
-  elif np.sqrt(delta) + v_rel < 0.1:
+  elif math.sqrt(delta) + v_rel < 0.1:
     ttc = 5.
   else:
-    ttc = 2 * d_rel / (np.sqrt(delta) + v_rel)
+    ttc = 2 * d_rel / (math.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)
+  decel = 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_target[1] = min(a_target[1], decel)
+    a_target[0] = min(a_target[1], a_target[0])
     a_pcm = 0.
   return a_target, a_pcm
 
@@ -258,7 +258,10 @@ def compute_speed_with_leads(v_ego, angle_steers, v_pid, l1, l2, awareness_statu
   v_target_lead = MAX_SPEED_POSSIBLE
 
   #*** set accel limits as cruise accel/decel limits ***
-  a_target, a_pcm = calc_cruise_accel_limits(v_ego)
+  a_target = calc_cruise_accel_limits(v_ego)
+  # Always 1 for now.
+  a_pcm = 1
+
   #*** 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.

selfdrive/controls/lib/alert_database.py

@@ -1,178 +0,0 @@
-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/alertmanager.py

@@ -0,0 +1,113 @@
+from cereal import car
+
+class ET:
+  ENABLE = 0
+  NO_ENTRY = 1
+  WARNING = 2
+  SOFT_DISABLE = 3
+  IMMEDIATE_DISABLE = 4
+  USER_DISABLE = 5
+
+class alert(object):
+  def __init__(self, alert_text_1, alert_text_2, alert_type, visual_alert, audible_alert, duration_sound, duration_hud_alert, duration_text):
+    self.alert_text_1 = alert_text_1
+    self.alert_text_2 = alert_text_2
+    self.alert_type = alert_type
+    self.visual_alert = visual_alert if visual_alert is not None else "none"
+    self.audible_alert = audible_alert if audible_alert is not None else "none"
+
+    self.duration_sound = duration_sound
+    self.duration_hud_alert = duration_hud_alert
+    self.duration_text = duration_text
+
+    # typecheck that enums are valid on startup
+    tst = car.CarControl.new_message()
+    tst.hudControl.visualAlert = self.visual_alert
+    tst.hudControl.audibleAlert = self.audible_alert
+  
+  def __str__(self):
+    return self.alert_text_1 + "/" + self.alert_text_2 + " " + str(self.alert_type) + "  " + str(self.visual_alert) + " " + str(self.audible_alert)
+
+  def __gt__(self, alert2):
+    return self.alert_type > alert2.alert_type
+
+class AlertManager(object):
+  alerts = {
+    "enable":             alert("", "",                                            ET.ENABLE,       None,               "beepSingle", .2, 0., 0.),
+    "disable":            alert("", "",                                            ET.USER_DISABLE, None,               "beepSingle", .2, 0., 0.),
+    "pedalPressed":       alert("Comma Unavailable",        "Pedal Pressed",       ET.NO_ENTRY,     "brakePressed",     "chimeDouble", .4, 2., 3.),
+    "driverDistracted":   alert("Take Control to Regain Speed", "User Distracted", ET.WARNING,      "steerRequired", "chimeRepeated", 1., 1., 1.),
+    "steerSaturated":     alert("Take Control", "Steer Control Saturated",         ET.WARNING,      "steerRequired", "chimeSingle", 1., 2., 3.),
+    "overheat":           alert("Take Control Immediately", "System Overheated",   ET.SOFT_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
+    "controlsMismatch":   alert("Take Control Immediately", "Controls Mismatch",   ET.IMMEDIATE_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
+    "radarCommIssue":     alert("Take Control Immediately", "Radar Comm Issue",    ET.IMMEDIATE_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
+    "modelCommIssue":     alert("Take Control Immediately", "Model Comm Issue",    ET.IMMEDIATE_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
+    "controlsFailed":     alert("Take Control Immediately", "Controls Failed",     ET.IMMEDIATE_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
+    # car errors
+    "commIssue":          alert("Take Control Immediately","Communication Issues", ET.IMMEDIATE_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
+    "steerUnavailable":   alert("Take Control Immediately","Steer Error",          ET.IMMEDIATE_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
+    "steerTemporarilyUnavailable": alert("Take Control", "Steer Temporarily Unavailable",  ET.WARNING,   "steerRequired", "chimeRepeated", 1., 2., 3.),
+    "brakeUnavailable":   alert("Take Control Immediately","Brake Error",          ET.IMMEDIATE_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
+    "gasUnavailable":     alert("Take Control Immediately","Gas Error",            ET.IMMEDIATE_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
+    "wrongGear":          alert("Take Control Immediately","Gear not D",           ET.SOFT_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
+    "reverseGear":        alert("Take Control Immediately","Car in Reverse",       ET.IMMEDIATE_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
+    "doorOpen":           alert("Take Control Immediately","Door Open",            ET.SOFT_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
+    "seatbeltNotLatched": alert("Take Control Immediately","Seatbelt Unlatched",   ET.SOFT_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
+    "espDisabled":        alert("Take Control Immediately","ESP Off",              ET.SOFT_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
+    "wrongCarMode":       alert("Comma Unavailable","Main Switch Off",      ET.NO_ENTRY,     None, None, .4, 0., 3.),
+  }
+  def __init__(self):
+    self.activealerts = []
+    self.current_alert = None
+
+  def alertPresent(self):
+    return len(self.activealerts) > 0
+
+  def alertShouldSoftDisable(self):
+    return len(self.activealerts) > 0 and self.activealerts[0].alert_type == ET.SOFT_DISABLE
+
+  def alertShouldDisable(self):
+    return len(self.activealerts) > 0 and self.activealerts[0].alert_type >= ET.IMMEDIATE_DISABLE
+
+  def add(self, alert_type, enabled = True):
+    this_alert = self.alerts[str(alert_type)]
+
+    # downgrade the alert if we aren't enabled
+    if not enabled and this_alert.alert_type > ET.NO_ENTRY:
+      this_alert = alert("Comma Unavailable" if this_alert.alert_text_1 != "" else "", this_alert.alert_text_2, ET.NO_ENTRY, None, "chimeDouble", .4, 0., 3.)
+
+    # ignore no entries if we are enabled 
+    if enabled and this_alert.alert_type < ET.WARNING:
+      return
+
+    self.activealerts.append(this_alert)
+    self.activealerts.sort()
+
+  def process_alerts(self, cur_time):
+    if self.alertPresent():
+      self.alert_start_time = cur_time
+      self.current_alert = self.activealerts[0]
+      print self.current_alert
+
+    alert_text_1 = ""
+    alert_text_2 = ""
+    visual_alert = "none"
+    audible_alert = "none"
+
+    if self.current_alert is not None:
+      # ewwwww
+      if self.alert_start_time + self.current_alert.duration_sound > cur_time:
+        audible_alert = self.current_alert.audible_alert
+
+      if self.alert_start_time + self.current_alert.duration_hud_alert > cur_time:
+        visual_alert = self.current_alert.visual_alert
+
+      if self.alert_start_time + self.current_alert.duration_text > cur_time:
+        alert_text_1 = self.current_alert.alert_text_1
+        alert_text_2 = self.current_alert.alert_text_2
+
+    # reset
+    self.activealerts = []
+
+    return alert_text_1, alert_text_2, visual_alert, audible_alert
+

selfdrive/controls/lib/drive_helpers.py

@@ -1,7 +1,8 @@
 import numpy as np
+from common.numpy_fast import clip, interp
 
 def rate_limit(new_value, last_value, dw_step, up_step):
-  return np.clip(new_value, last_value + dw_step, last_value + up_step)
+  return 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
@@ -11,12 +12,12 @@ def learn_angle_offset(lateral_control, v_ego, angle_offset, d_poly, y_des, stee
   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))
+  alpha_v = alpha*(max(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)
+    angle_offset = clip(angle_offset, min_offset, max_offset)
 
   return angle_offset
 
@@ -44,7 +45,7 @@ def actuator_hystereses(final_brake, braking, brake_steady, v_ego, 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_on_offset = 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

selfdrive/controls/lib/latcontrol.py

@@ -1,4 +1,6 @@
+import math
 import numpy as np
+from common.numpy_fast import clip
 
 def calc_curvature(v_ego, angle_steers, VP, angle_offset=0):
   deg_to_rad = np.pi/180.
@@ -14,7 +16,7 @@ def calc_d_lookahead(v_ego):
   # 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
+  d_lookahead = offset_lookahead + math.sqrt(max(v_ego, 0)) * coeff_lookahead
   return d_lookahead
 
 def calc_lookahead_offset(v_ego, angle_steers, d_lookahead, VP, angle_offset):
@@ -53,7 +55,7 @@ def pid_lateral_control(v_ego, y_actual, y_des, Ui_steer, steer_max,
     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)
+  Ui_steer = clip(Ui_steer, -steer_max, steer_max)
 
   output_steer = Up_steer + Ui_steer
 
@@ -67,7 +69,7 @@ def pid_lateral_control(v_ego, y_actual, y_des, Ui_steer, steer_max,
   if abs(output_steer) > steer_max:
     lateral_control_sat = True
 
-  output_steer = np.clip(output_steer, -steer_max, steer_max)
+  output_steer = 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
@@ -81,7 +83,7 @@ def pid_lateral_control(v_ego, y_actual, y_des, Ui_steer, steer_max,
   if sat_count >= sat_count_limit:
     sat_flag = True
 
-  sat_count = np.clip(sat_count, 0, 1)
+  sat_count = clip(sat_count, 0, 1)
 
   return output_steer, Up_steer, Ui_steer, lateral_control_sat, sat_count, sat_flag
 
@@ -97,24 +99,24 @@ class LatControl(object):
   def reset(self):
     self.Ui_steer = 0.
 
-  def update(self, enabled, CS, d_poly, angle_offset):
+  def update(self, enabled, v_ego, angle_steers, steer_override, d_poly, angle_offset, VP):
     rate = 100
 
     steer_max = 1.0
 
     # how far we look ahead is function of speed
-    d_lookahead = calc_d_lookahead(CS.v_ego)
+    d_lookahead = calc_d_lookahead(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)
+    self.y_actual, _ = calc_lookahead_offset(v_ego, angle_steers,
+                                             d_lookahead, 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)
+      v_ego, self.y_actual, self.y_des, self.Ui_steer, steer_max,
+      steer_override, self.sat_count, enabled, VP.torque_mod, rate)
 
-    final_steer = np.clip(output_steer, -steer_max, steer_max)
+    final_steer = clip(output_steer, -steer_max, steer_max)
     return final_steer, sat_flag

selfdrive/controls/lib/longcontrol.py

@@ -1,4 +1,5 @@
 import numpy as np
+from common.numpy_fast import clip, interp
 from selfdrive.config import Conversions as CV
 
 class LongCtrlState:
@@ -82,15 +83,18 @@ def get_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()
 
+
+_KP_BP = [0., 5., 35.]
+_KP_V =  [1.2, 0.8, 0.5]
+
+_kI_BP = [0., 35.]
+_kI_V =  [0.18, 0.12]
+
 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)
+  Kp = interp(v_ego, _KP_BP, _KP_V)
+  Ki = interp(v_ego, _kI_BP, _kI_V)
 
   # scle Kp and Ki by jerk factor drom drive_thread
   Kp = (1. + jerk_factor)*Kp
@@ -98,7 +102,7 @@ def pid_long_control(v_ego, v_pid, Ui_accel_cmd, gas_max, brake_max, jerk_factor
 
   # 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_ego = max(v_ego, v_ego_min)
 
   v_error = v_pid - v_ego
 
@@ -107,7 +111,7 @@ def pid_long_control(v_ego, v_pid, Ui_accel_cmd, gas_max, brake_max, jerk_factor
   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
+  # Anti-wind up for integrator: only update integrator if we not against the throttle 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
@@ -126,7 +130,7 @@ def pid_long_control(v_ego, v_pid, Ui_accel_cmd, gas_max, brake_max, jerk_factor
   if output_gb > gas_max or output_gb < -brake_max:
     long_control_sat = True
 
-  output_gb = np.clip(output_gb, -brake_max, gas_max)
+  output_gb = clip(output_gb, -brake_max, gas_max)
 
   return output_gb, Up_accel_cmd, Ui_accel_cmd, long_control_sat
 
@@ -136,8 +140,8 @@ 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
+_MAX_SPEED_ERROR_BP = [0., 30.]  # speed breakpoints
+_MAX_SPEED_ERROR_V  = [1.5, .8]  # max positive v_pid error VS actual speed; this avoids controls windup due to slow pedal resp
 
 class LongControl(object):
   def __init__(self):
@@ -151,19 +155,20 @@ class LongControl(object):
     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
+  def update(self, enabled, v_ego, v_cruise, v_target_lead, a_target, jerk_factor, VP):
     brake_max_bp = [0., 5., 20., 100.]  # speeds     
+    brake_max_v = [1.0, 1.0, 0.8, 0.8]  # values
 
     # 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)
+    brake_max = interp(v_ego, brake_max_bp, brake_max_v)
+
+    # TODO: not every time
+    if VP.brake_only:
+      gas_max = 0
+    else:
+      gas_max_bp = [0., 100.]             # speeds
+      gas_max_v = [0.6, 0.6]            # values
+      gas_max = interp(v_ego, gas_max_bp, gas_max_v)
 
     overshoot_allowance = 2.0    # overshoot allowed when changing accel sign
 
@@ -172,30 +177,30 @@ class LongControl(object):
 
     # 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)
+    v_target = min(v_target_lead, v_cruise_mph * CV.MPH_TO_MS / VP.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)
+    self.long_control_state = long_control_state_trans(enabled, self.long_control_state, 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
+      self.v_pid = 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
+      if ((self.v_pid > 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
+        self.v_pid = max(v_target, v_ego + overshoot_allowance)
+      elif ((self.v_pid < v_ego - overshoot_allowance) and
           (v_target > self.v_pid)):
-        self.v_pid = np.minimum(v_target, CS.v_ego - overshoot_allowance)
+        self.v_pid = min(v_target, v_ego - overshoot_allowance)
 
       # move v_pid no faster than allowed accel limits
       if (v_target > self.v_pid + max_speed_delta_up):
@@ -206,27 +211,29 @@ class LongControl(object):
         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)
+      if not VP.brake_only:
+        max_speed_error = interp(v_ego, _MAX_SPEED_ERROR_BP, _MAX_SPEED_ERROR_V)
+        self.v_pid = min(self.v_pid, 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)
+      # TODO: removed anti windup on gear change, does it matter?
+      output_gb, self.Up_accel_cmd, self.Ui_accel_cmd, self.long_control_sat = pid_long_control(v_ego, self.v_pid, \
+                                  self.Ui_accel_cmd, gas_max, brake_max, jerk_factor, 0, 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:
+      if v_ego > 0. or output_gb > -brake_stopping_target:
         output_gb -= stopping_brake_rate/rate
-      output_gb = np.clip(output_gb, -brake_max, gas_max)
-      self.v_pid = CS.v_ego
+      output_gb = clip(output_gb, -brake_max, gas_max)
+      self.v_pid = 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.v_pid = 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.)
+    final_gas = clip(output_gb, 0., gas_max)
+    final_brake = -clip(output_gb, -brake_max, 0.)
     return final_gas, final_brake
+

selfdrive/controls/lib/pathplanner.py

@@ -1,26 +1,29 @@
-import selfdrive.messaging as messaging
+import math
 import numpy as np
+
+from common.numpy_fast import interp
+import selfdrive.messaging as messaging
 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])
+_LANE_WIDTH_V = [3., 3.8]
 
 # break points of speed
-_LANE_WIDTH_BP = np.asarray([0., 31.])
+_LANE_WIDTH_BP = [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)
+  lane_width = 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.)
+    c_prob = math.sqrt((l_prob**2 + r_prob**2) / 2.)
   else:
     c_poly = np.zeros(4)
     c_prob = 0.

selfdrive/controls/lib/radar_helpers.py

@@ -1,8 +1,10 @@
-import numpy as np
-import platform
 import os
 import sys
+import math
+import platform
+import numpy as np
 
+from common.numpy_fast import clip, interp
 from common.kalman.ekf import FastEKF1D, SimpleSensor
 
 # radar tracks
@@ -51,14 +53,14 @@ class Track(object):
     else:
       # estimate acceleration
       a_rel_unfilt = (self.vRel - self.vRelPrev) / ts
-      a_rel_unfilt = np.clip(a_rel_unfilt, -10., 10.)
+      a_rel_unfilt = 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.)
+      a_lead_unfilt = clip(a_lead_unfilt, -10., 10.)
       self.aLead = k_a_lead * a_lead_unfilt + (1 - k_a_lead) * self.aLead
 
     if self.stationary:
@@ -232,12 +234,12 @@ class Cluster(object):
     d_path = self.dPath
 
     if enabled:
-      t_lookahead = np.interp(self.dRel, t_lookahead_bp, t_lookahead_v)
+      t_lookahead = 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)
+      lat_corr = clip(t_lookahead * self.vLat, -1, 0)
     else:
       lat_corr = 0.
-    d_path = np.maximum(d_path + lat_corr, 0)
+    d_path = max(d_path + lat_corr, 0)
 
     if d_path < 1.5 and not self.stationary and not self.oncoming:
       return True

selfdrive/controls/radard.py

@@ -1,4 +1,5 @@
 #!/usr/bin/env python
+import os
 import zmq
 import numpy as np
 import numpy.matlib
@@ -7,9 +8,7 @@ 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
@@ -18,10 +17,16 @@ from common.services import service_list
 from common.realtime import sec_since_boot, set_realtime_priority, Ratekeeper
 from common.kalman.ekf import EKF, SimpleSensor
 
+radar_type = os.getenv("RADAR")
+if radar_type is not None:
+  exec('from selfdrive.radar.'+car_type+'.interface import RadarInterface')
+else:
+  from selfdrive.radar.nidec.interface import RadarInterface
+
 #vision point
 DIMSV = 2
 XV, SPEEDV = 0, 1
-VISION_POINT = 1
+VISION_POINT = -1
 
 class EKFV1D(EKF):
   def __init__(self):
@@ -31,7 +36,6 @@ class EKFV1D(EKF):
     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):
@@ -41,30 +45,6 @@ class EKFV1D(EKF):
     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)
@@ -73,10 +53,10 @@ def radard_thread(gctx=None):
 
   # *** 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)
+  RI = RadarInterface()
 
   # *** publish live20 and liveTracks
   live20 = messaging.pub_sock(context, service_list['live20'].port)
@@ -84,9 +64,8 @@ def radard_thread(gctx=None):
 
   # subscribe to stats about the car
   # TODO: move this to new style packet
-  VP = VehicleParams(False)  # same for ILX and civic
+  VP = VehicleParams(False, False)  # same for ILX and civic
 
-  ar_pts = {}
   path_x = np.arange(0.0, 140.0, 0.1)    # 140 meters is max
 
   # Time-alignment
@@ -100,9 +79,6 @@ def radard_thread(gctx=None):
 
   tracks = defaultdict(dict)
 
-  # Nidec
-  cp = _create_radard_can_parser()
-
   # Kalman filter stuff: 
   ekfv = EKFV1D()
   speedSensorV = SimpleSensor(XV, 1, 2)
@@ -114,23 +90,11 @@ def radard_thread(gctx=None):
 
   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]))
+    rr = RI.update()
 
-    # 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)
+    ar_pts = {}
+    for pt in rr.points:
+      ar_pts[pt.trackId] = [pt.dRel + RDR_TO_LDR, pt.yRel, pt.vRel, pt.aRel, None, False, None]
 
     # receive the live100s
     l100 = messaging.recv_sock(live100)
@@ -184,7 +148,7 @@ def radard_thread(gctx=None):
       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
+      # create the track if it doesn't exist or it's a new 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)
@@ -246,7 +210,7 @@ def radard_thread(gctx=None):
     dat = messaging.new_message()
     dat.init('live20')
     dat.live20.mdMonoTime = PP.logMonoTime
-    dat.live20.canMonoTimes = canMonoTimes
+    dat.live20.canMonoTimes = list(rr.canMonoTimes)
     if lead_len > 0:
       lead_clusters[0].toLive20(dat.live20.leadOne)
       if lead2_len > 0:

selfdrive/loggerd/uploader.py

@@ -84,6 +84,8 @@ class Uploader(object):
       cloudlog.exception("clean_dirs failed")
 
   def gen_upload_files(self):
+    if not os.path.isdir(self.root):
+      return
     for logname in listdir_by_creation_date(self.root):
       path = os.path.join(self.root, logname)
       names = os.listdir(path)

selfdrive/manager.py

@@ -29,7 +29,6 @@ managed_processes = {
   "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"]),
@@ -38,7 +37,13 @@ managed_processes = {
 
 running = {}
 
-car_started_processes = ['controlsd', 'loggerd', 'visiond', 'sensord', 'radard', 'calibrationd']
+# due to qualcomm kernel bugs SIGKILLing visiond sometimes causes page table corruption
+unkillable_processes = ['visiond']
+
+# processes to end with SIGINT instead of SIGTERM
+interrupt_processes = ['loggerd']
+
+car_started_processes = ['controlsd', 'loggerd', 'sensord', 'radard', 'calibrationd', 'visiond']
 
 
 # ****************** process management functions ******************
@@ -91,15 +96,30 @@ 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()
+
+  if name in interrupt_processes:
+    os.kill(running[name].pid, signal.SIGINT)
+  else:
+    running[name].terminate()
+
+
+  # give it 5 seconds to die
   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))
+    if name in unkillable_processes:
+      cloudlog.critical("unkillable process %s failed to exit! rebooting in 15 if it doesn't die" % name)
+      running[name].join(15.0)
+      if running[name].exitcode is None:
+        cloudlog.critical("FORCE REBOOTING PHONE!")
+        os.system("date > /sdcard/unkillable_reboot")
+        os.system("reboot")
+        raise RuntimeError
+    else:
+      cloudlog.info("killing %s with SIGKILL" % name)
+      os.kill(running[name].pid, signal.SIGKILL)
+      running[name].join()
+
+  cloudlog.info("%s is dead with %d" % (name, running[name].exitcode))
   del running[name]
 
 def cleanup_all_processes(signal, frame):
@@ -125,6 +145,7 @@ def manager_init():
   os.environ['DONGLE_SECRET'] = dongle_secret
 
   cloudlog.bind_global(dongle_id=dongle_id)
+  common.crash.bind_user(dongle_id=dongle_id)
 
   # set gctx
   gctx = {
@@ -152,8 +173,9 @@ def manager_thread():
   start_managed_process("uploader")
   start_managed_process("ui")
 
-  # *** wait for the board ***
-  wait_for_device()
+  if os.getenv("NOBOARD") is None:
+    # *** wait for the board ***
+    wait_for_device()
 
   # flash the device
   if os.getenv("NOPROG") is None:
@@ -188,12 +210,13 @@ def manager_thread():
       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)
+    if not os.getenv("STARTALL"):
+      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:
@@ -257,8 +280,17 @@ def wait_for_device():
 def main():
   if os.getenv("NOLOG") is not None:
     del managed_processes['loggerd']
+  if os.getenv("NOUPLOAD") is not None:
+    del managed_processes['uploader']
+  if os.getenv("NOVISION") is not None:
+    del managed_processes['visiond']
   if os.getenv("NOBOARD") is not None:
     del managed_processes['boardd']
+  if os.getenv("LEAN") is not None:
+    del managed_processes['uploader']
+    del managed_processes['loggerd']
+    del managed_processes['logmessaged']
+    del managed_processes['logcatd']
 
   manager_init()
 

selfdrive/radar/nidec/interface.py

@@ -0,0 +1,83 @@
+#!/usr/bin/env python
+import numpy as np
+from selfdrive.car.honda.can_parser import CANParser
+from selfdrive.boardd.boardd import can_capnp_to_can_list
+
+from cereal import car
+
+import zmq
+from common.services import service_list
+import selfdrive.messaging as messaging
+
+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)
+
+class RadarInterface(object):
+  def __init__(self):
+    # radar
+    self.pts = {}
+    self.track_id = 0
+
+    # Nidec
+    self.rcp = _create_radard_can_parser()
+
+    context = zmq.Context()
+    self.logcan = messaging.sub_sock(context, service_list['can'].port)
+
+  def update(self):
+    canMonoTimes = []
+    can_pub_radar = []
+
+    # TODO: can hang if no packets show up
+    while 1:
+      for a in messaging.drain_sock(self.logcan, wait_for_one=True):
+        canMonoTimes.append(a.logMonoTime)
+        can_pub_radar.extend(can_capnp_to_can_list(a.can, [1, 3]))
+
+      # only run on the 0x445 packets, used for timing
+      if any(x[0] == 0x445 for x in can_pub_radar):
+        break
+
+    self.rcp.update_can(can_pub_radar)
+
+    ret = car.RadarState.new_message()
+    errors = []
+    if not self.rcp.can_valid:
+      errors.append("notValid")
+    ret.errors = errors
+    ret.canMonoTimes = canMonoTimes
+
+    for ii in self.rcp.msgs_upd:
+      cpt = self.rcp.vl[ii]
+      if cpt['LONG_DIST'] < 255:
+        if ii not in self.pts or cpt['NEW_TRACK']:
+          self.pts[ii] = car.RadarState.RadarPoint.new_message()
+          self.pts[ii].trackId = self.track_id
+          self.track_id += 1
+        self.pts[ii].dRel = cpt['LONG_DIST']  # from front of car
+        self.pts[ii].yRel = -cpt['LAT_DIST']  # in car frame's y axis, left is positive
+        self.pts[ii].vRel = cpt['REL_SPEED']
+        self.pts[ii].aRel = float('nan')
+        self.pts[ii].yvRel = float('nan')
+      else:
+        if ii in self.pts:
+          del self.pts[ii]
+
+    ret.points = self.pts.values()
+    return ret
+
+if __name__ == "__main__":
+  RI = RadarInterface()
+  while 1:
+    ret = RI.update()
+    print(chr(27) + "[2J")
+    print ret
+
+

selfdrive/test/plant/maneuver.py

@@ -0,0 +1,71 @@
+import os
+from enum import Enum
+from maneuverplots import ManeuverPlot
+from plant import Plant
+import numpy as np
+
+
+class Maneuver(object):
+  def __init__(self, title, duration, **kwargs):
+    # Was tempted to make a builder class
+    self.distance_lead = kwargs.get("initial_distance_lead", 200.0)
+    self.speed = kwargs.get("initial_speed", 0.0)
+    self.lead_relevancy = kwargs.get("lead_relevancy", 0) 
+
+    self.grade_values = kwargs.get("grade_values", [0.0, 0.0])
+    self.grade_breakpoints = kwargs.get("grade_breakpoints", [0.0, duration])
+    self.speed_lead_values = kwargs.get("speed_lead_values", [0.0, 0.0])
+    self.speed_lead_breakpoints = kwargs.get("speed_lead_values", [0.0, duration])
+
+    self.cruise_button_presses = kwargs.get("cruise_button_presses", [])
+
+    self.duration = duration
+    self.title = title
+
+  def evaluate(self):
+    """runs the plant sim and returns (score, run_data)"""
+    plant = Plant(
+      lead_relevancy = self.lead_relevancy,
+      speed = self.speed,
+      distance_lead = self.distance_lead
+    )
+
+    last_live100 = None
+    event_queue = sorted(self.cruise_button_presses, key=lambda a: a[1])[::-1]
+    plot = ManeuverPlot(self.title)
+
+    buttons_sorted = sorted(self.cruise_button_presses, key=lambda a: a[1])
+    current_button = 0
+    while plant.current_time() < self.duration:
+      while buttons_sorted and plant.current_time() >= buttons_sorted[0][1]:
+        current_button = buttons_sorted[0][0]
+        buttons_sorted = buttons_sorted[1:]
+        print "current button changed to", current_button
+    
+      grade = np.interp(plant.current_time(), self.grade_breakpoints, self.grade_values)
+      speed_lead = np.interp(plant.current_time(), self.speed_lead_breakpoints, self.speed_lead_values)
+
+      distance, speed, acceleration, distance_lead, brake, gas, steer_torque, live100 = plant.step(speed_lead, current_button, grade)
+      if live100:
+        last_live100 = live100[-1]
+
+      d_rel = distance_lead - distance if self.lead_relevancy else 200. 
+      v_rel = speed_lead - speed if self.lead_relevancy else 0. 
+
+      if last_live100:
+        # print last_live100
+        #develop plots
+        plot.add_data(
+          time=plant.current_time(),
+          gas=gas, brake=brake, steer_torque=steer_torque,
+          distance=distance, speed=speed, acceleration=acceleration,
+          up_accel_cmd=last_live100.upAccelCmd, ui_accel_cmd=last_live100.uiAccelCmd,
+          d_rel=d_rel, v_rel=v_rel, v_lead=speed_lead,
+          v_target_lead=last_live100.vTargetLead, pid_speed=last_live100.vPid,
+          cruise_speed=last_live100.vCruise,
+          jerk_factor=last_live100.jerkFactor,
+          a_target_min=last_live100.aTargetMin, a_target_max=last_live100.aTargetMax)
+    
+    return (None, plot)
+
+

selfdrive/test/plant/maneuverplots.py

@@ -0,0 +1,140 @@
+import os
+
+import numpy as np
+import matplotlib.pyplot as plt
+import pylab
+
+from selfdrive.config import Conversions as CV
+
+class ManeuverPlot(object):
+  def __init__(self, title = None):
+    self.time_array = []
+
+    self.gas_array =  []
+    self.brake_array = []
+    self.steer_torque_array = []
+
+    self.distance_array = []
+    self.speed_array = []
+    self.acceleration_array = []
+
+    self.up_accel_cmd_array = []
+    self.ui_accel_cmd_array = []
+
+    self.d_rel_array = []
+    self.v_rel_array = []
+    self.v_lead_array = []
+    self.v_target_lead_array = []
+    self.pid_speed_array = []
+    self.cruise_speed_array = []
+    self.jerk_factor_array = []
+
+    self.a_target_min_array = []
+    self.a_target_max_array = []
+
+    self.v_target_array = []
+
+    self.title = title
+    
+  def add_data(self, time, gas, brake, steer_torque, distance, speed, 
+    acceleration, up_accel_cmd, ui_accel_cmd, d_rel, v_rel, v_lead, 
+    v_target_lead, pid_speed, cruise_speed, jerk_factor, a_target_min, 
+    a_target_max):
+    self.time_array.append(time)
+    self.gas_array.append(gas)
+    self.brake_array.append(brake)
+    self.steer_torque_array.append(steer_torque)
+    self.distance_array.append(distance)
+    self.speed_array.append(speed)
+    self.acceleration_array.append(acceleration)
+    self.up_accel_cmd_array.append(up_accel_cmd)
+    self.ui_accel_cmd_array.append(ui_accel_cmd)
+    self.d_rel_array.append(d_rel)
+    self.v_rel_array.append(v_rel)
+    self.v_lead_array.append(v_lead)
+    self.v_target_lead_array.append(v_target_lead)
+    self.pid_speed_array.append(pid_speed)
+    self.cruise_speed_array.append(cruise_speed)
+    self.jerk_factor_array.append(jerk_factor)
+    self.a_target_min_array.append(a_target_min)
+    self.a_target_max_array.append(a_target_max)
+
+
+  def write_plot(self, path, maneuver_name):
+    title = self.title or maneuver_name
+    # TODO: Missing plots from the old one:
+    # long_control_state
+    # proportional_gb, intergral_gb
+    if not os.path.exists(path + "/" + maneuver_name):
+      os.makedirs(path + "/" + maneuver_name)
+    plt_num = 0
+    
+    # speed chart ===================
+    plt_num += 1
+    plt.figure(plt_num)
+    plt.plot(
+      np.array(self.time_array), np.array(self.speed_array) * CV.MS_TO_MPH, 'r',
+      np.array(self.time_array), np.array(self.pid_speed_array) * CV.MS_TO_MPH, 'y--',
+      np.array(self.time_array), np.array(self.v_target_lead_array) * CV.MS_TO_MPH, 'b',
+      np.array(self.time_array), np.array(self.cruise_speed_array) * CV.KPH_TO_MPH, 'k',
+      np.array(self.time_array), np.array(self.v_lead_array) * CV.MS_TO_MPH, 'm'
+    )
+    plt.xlabel('Time [s]')
+    plt.ylabel('Speed [mph]')
+    plt.legend(['speed', 'pid speed', 'Target (lead) speed', 'Cruise speed', 'Lead speed'], loc=0)
+    plt.grid()
+    pylab.savefig("/".join([path, maneuver_name, 'speeds.svg']), dpi=1000)
+
+    # acceleration chart ============
+    plt_num += 1
+    plt.figure(plt_num)
+    plt.plot(
+      np.array(self.time_array), np.array(self.acceleration_array), 'g',
+      np.array(self.time_array), np.array(self.a_target_min_array), 'k--',
+      np.array(self.time_array), np.array(self.a_target_max_array), 'k--',
+    )
+    plt.xlabel('Time [s]')
+    plt.ylabel('Acceleration [m/s^2]')
+    plt.legend(['accel', 'max', 'min'], loc=0)
+    plt.grid()
+    pylab.savefig("/".join([path, maneuver_name, 'acceleration.svg']), dpi=1000)
+
+    # pedal chart ===================
+    plt_num += 1
+    plt.figure(plt_num)
+    plt.plot(
+      np.array(self.time_array), np.array(self.gas_array), 'g',
+      np.array(self.time_array), np.array(self.brake_array), 'r',
+    )
+    plt.xlabel('Time [s]')
+    plt.ylabel('Pedal []')
+    plt.legend(['Gas pedal', 'Brake pedal'], loc=0)
+    plt.grid()
+    pylab.savefig("/".join([path, maneuver_name, 'pedals.svg']), dpi=1000)
+
+    # pid chart ======================
+    plt_num += 1
+    plt.figure(plt_num)
+    plt.plot(
+      np.array(self.time_array), np.array(self.up_accel_cmd_array), 'g',
+      np.array(self.time_array), np.array(self.ui_accel_cmd_array), 'b'
+    )
+    plt.xlabel("Time, [s]")
+    plt.ylabel("Accel Cmd [m/s^2]")
+    plt.grid()
+    plt.legend(["Proportional", "Integral"], loc=0)
+    pylab.savefig("/".join([path, maneuver_name, "pid.svg"]), dpi=1000)
+
+    # relative distances chart =======
+    plt_num += 1
+    plt.figure(plt_num)
+    plt.plot(
+      np.array(self.time_array), np.array(self.d_rel_array), 'g',
+    )
+    plt.xlabel('Time [s]')
+    plt.ylabel('Relative Distance [m]')
+    plt.grid()
+    pylab.savefig("/".join([path, maneuver_name, 'distance.svg']), dpi=1000)
+
+    plt.close("all")
+

selfdrive/test/plant/plant.py

@@ -0,0 +1,274 @@
+#!/usr/bin/env python
+import os
+import struct
+
+import zmq
+import numpy as np
+
+from dbcs import DBC_PATH
+
+from common.realtime import Ratekeeper
+from common.services import service_list
+
+import selfdrive.messaging as messaging
+from selfdrive.config import CruiseButtons
+from selfdrive.car.honda.hondacan import fix
+from selfdrive.car.honda.carstate import get_can_parser
+from selfdrive.boardd.boardd import can_capnp_to_can_list, can_list_to_can_capnp
+
+from selfdrive.car.honda.can_parser import CANParser
+
+
+from common.dbc import dbc
+acura = dbc(os.path.join(DBC_PATH, "acura_ilx_2016_can.dbc"))
+
+def car_plant(pos, speed, grade, gas, brake):
+  # vehicle parameters
+  mass = 1700
+  aero_cd = 0.3
+  force_peak = mass*3.
+  force_brake_peak = -mass*10.     #1g
+  power_peak = 100000   # 100kW
+  speed_base = power_peak/force_peak
+  rolling_res = 0.01
+  g = 9.81
+  wheel_r = 0.31
+  frontal_area = 2.2
+  air_density = 1.225
+  gas_to_peak_linear_slope = 3.33
+  brake_to_peak_linear_slope = 0.2
+  creep_accel_v = [1., 0.]
+  creep_accel_bp = [0., 1.5]
+
+  #*** longitudinal model ***
+  # find speed where peak torque meets peak power
+  force_brake = brake * force_brake_peak * brake_to_peak_linear_slope
+  if speed < speed_base: # torque control
+    force_gas = gas * force_peak * gas_to_peak_linear_slope
+  else: # power control
+    force_gas = gas * power_peak / speed * gas_to_peak_linear_slope
+
+  force_grade = - grade * mass  # positive grade means uphill
+
+  creep_accel = np.interp(speed, creep_accel_bp, creep_accel_v)
+  force_creep = creep_accel * mass
+
+  force_resistance = -(rolling_res * mass * g + 0.5 * speed**2 * aero_cd * air_density)
+  force = force_gas + force_brake + force_resistance + force_grade + force_creep
+  acceleration = force / mass
+
+  # TODO: lateral model
+  return speed, acceleration
+
+def get_car_can_parser():
+  dbc_f = 'acura_ilx_2016_can.dbc'
+  signals = [
+    ("STEER_TORQUE", 0xe4, 0),
+    ("STEER_TORQUE_REQUEST", 0xe4, 0),
+    ("COMPUTER_BRAKE", 0x1fa, 0),
+    ("COMPUTER_BRAKE_REQUEST", 0x1fa, 0),
+    ("GAS_COMMAND", 0x200, 0),
+  ]
+  checks = [
+    (0xe4, 100),
+    (0x1fa, 50),
+    (0x200, 50),
+  ]
+  return CANParser(dbc_f, signals, checks)
+
+
+class Plant(object):
+  messaging_initialized = False
+
+  def __init__(self, lead_relevancy=False, rate=100, speed=0.0, distance_lead=2.0):
+    self.rate = rate
+    self.civic = False
+    self.brake_only = False
+
+    if not Plant.messaging_initialized:
+      context = zmq.Context()
+      Plant.logcan = messaging.pub_sock(context, service_list['can'].port)
+      Plant.sendcan = messaging.sub_sock(context, service_list['sendcan'].port)
+      Plant.model = messaging.pub_sock(context, service_list['model'].port)
+      Plant.live100 = messaging.sub_sock(context, service_list['live100'].port)
+      Plant.messaging_initialized = True
+
+    self.angle_steer = 0.
+    self.gear_choice = 0
+    self.speed, self.speed_prev = 0., 0.
+
+    self.esp_disabled = 0
+    self.main_on = 1
+    self.user_gas = 0
+    self.computer_brake,self.user_brake = 0,0
+    self.brake_pressed = 0
+    self.distance, self.distance_prev = 0., 0.
+    self.speed, self.speed_prev = speed, speed
+    self.steer_error, self.brake_error, self.steer_not_allowed = 0, 0, 0
+    self.gear_shifter = 4   # D gear
+    self.pedal_gas = 0
+    self.cruise_setting = 0
+
+    self.seatbelt, self.door_all_closed = True, True
+    self.steer_torque, self.v_cruise, self.acc_status = 0, 0, 0  # v_cruise is reported from can, not the one used for controls
+
+    self.lead_relevancy = lead_relevancy
+
+    # lead car
+    self.distance_lead, self.distance_lead_prev = distance_lead , distance_lead
+
+    self.rk = Ratekeeper(rate, print_delay_threshold=100)
+    self.ts = 1./rate
+
+    self.cp = get_car_can_parser()
+
+  def speed_sensor(self, speed):
+    if speed<0.3:
+      return 0
+    else:
+      return speed
+
+  def current_time(self):
+    return float(self.rk.frame) / self.rate
+
+  def step(self, v_lead=0.0, cruise_buttons=None, grade=0.0):
+    # dbc_f, sgs, ivs, msgs, cks_msgs, frqs = initialize_can_struct(self.civic, self.brake_only)
+    cp2 = get_can_parser(self.civic, self.brake_only)
+    sgs = cp2._sgs
+    msgs = cp2._msgs
+    cks_msgs = cp2.msgs_ck
+
+    # ******** get messages sent to the car ********
+    can_msgs = []
+    for a in messaging.drain_sock(Plant.sendcan):
+      can_msgs.extend(can_capnp_to_can_list(a.sendcan, [0,2]))
+    self.cp.update_can(can_msgs)
+
+    # ******** get live100 messages for plotting ***
+    live_msgs = []
+    for a in messaging.drain_sock(Plant.live100):
+      live_msgs.append(a.live100)
+
+
+    if self.cp.vl[0x1fa]['COMPUTER_BRAKE_REQUEST']:
+      brake = self.cp.vl[0x1fa]['COMPUTER_BRAKE']
+    else:
+      brake = 0.0
+
+    if self.cp.vl[0x200]['GAS_COMMAND'] > 0:
+      gas = self.cp.vl[0x200]['GAS_COMMAND'] / 256.0
+    else:
+      gas = 0.0
+
+    if self.cp.vl[0xe4]['STEER_TORQUE_REQUEST']:
+      steer_torque = self.cp.vl[0xe4]['STEER_TORQUE']*1.0/0xf00
+    else:
+      steer_torque = 0.0
+
+    distance_lead = self.distance_lead_prev + v_lead * self.ts
+
+    # ******** run the car ********
+    speed, acceleration = car_plant(self.distance_prev, self.speed_prev, grade, gas, brake)
+    standstill = (speed == 0)
+    distance = self.distance_prev + speed * self.ts
+    speed = self.speed_prev + self.ts * acceleration
+    if speed <= 0:
+      speed = 0
+      acceleration = 0
+
+    # ******** lateral ********
+    self.angle_steer -= steer_torque
+
+    # *** radar model ***
+    if self.lead_relevancy:
+      d_rel = np.maximum(0., self.distance_lead - distance)
+      v_rel = v_lead - speed
+    else:
+      d_rel = 200.
+      v_rel = 0.
+      a_rel = 0
+    lateral_pos_rel = 0.
+
+    # print at 5hz
+    if (self.rk.frame%(self.rate/5)) == 0:
+      print "%6.2f m  %6.2f m/s  %6.2f m/s2   %.2f ang   gas: %.2f  brake: %.2f  steer: %5.2f     lead_rel: %6.2f m  %6.2f m/s" % (distance, speed, acceleration, self.angle_steer, gas, brake, steer_torque, d_rel, v_rel)
+
+    # ******** publish the car ********
+    vls = [self.speed_sensor(speed), self.speed_sensor(speed), self.speed_sensor(speed), self.speed_sensor(speed),
+           self.angle_steer, 0, self.gear_choice, speed!=0,
+           0, 0, 0, 0,
+           self.v_cruise, not self.seatbelt, self.seatbelt, self.brake_pressed,
+           self.user_gas, cruise_buttons, self.esp_disabled, 0,
+           self.user_brake, self.steer_error, self.speed_sensor(speed), self.brake_error,
+           self.brake_error, self.gear_shifter, self.main_on, self.acc_status,
+           self.pedal_gas, self.cruise_setting,
+           # left_blinker, right_blinker, counter
+           0,0,0,
+           # interceptor_gas
+           0,0]
+
+
+    # TODO: publish each message at proper frequency
+    can_msgs = []
+    for msg in set(msgs):
+      msg_struct = {}
+      indxs = [i for i, x in enumerate(msgs) if msg == msgs[i]]
+      for i in indxs:
+        msg_struct[sgs[i]] = vls[i]
+      if msg in cks_msgs:
+        msg_struct["COUNTER"] = self.rk.frame % 4
+
+      msg_data = acura.encode(msg, msg_struct)
+
+      if msg in cks_msgs:
+        msg_data = fix(msg_data, msg)
+
+      can_msgs.append([msg, 0, msg_data, 0])
+
+    # add the radar message
+    # TODO: use the DBC
+    def to_3_byte(x):
+      return struct.pack("!H", int(x)).encode("hex")[1:]
+
+    def to_3s_byte(x):
+      return struct.pack("!h", int(x)).encode("hex")[1:]
+    radar_msg = to_3_byte(d_rel*16.0) + \
+                to_3_byte(int(lateral_pos_rel*16.0)&0x3ff) + \
+                to_3s_byte(int(v_rel*32.0)) + \
+                "0f00000"
+    can_msgs.append([0x445, 0, radar_msg.decode("hex"), 1])
+    Plant.logcan.send(can_list_to_can_capnp(can_msgs).to_bytes())
+
+    # ******** publish a fake model going straight ********
+    md = messaging.new_message()
+    md.init('model')
+    md.model.frameId = 0
+    for x in [md.model.path, md.model.leftLane, md.model.rightLane]:
+      x.points = [0.0]*50
+      x.prob = 1.0
+      x.std = 1.0
+    # fake values?
+    Plant.model.send(md.to_bytes())
+
+
+    # ******** update prevs ********
+    self.speed_prev = speed
+    self.distance_prev = distance
+    self.distance_lead_prev = distance_lead
+
+    self.rk.keep_time()
+    return (distance, speed, acceleration, distance_lead, brake, gas, steer_torque, live_msgs)
+
+# simple engage in standalone mode
+def plant_thread(rate=100):
+  plant = Plant(rate)
+  while 1:
+    if plant.rk.frame%100 >= 20 and plant.rk.frame%100 <= 25:
+      cruise_buttons = CruiseButtons.RES_ACCEL
+    else:
+      cruise_buttons = 0
+    plant.step()
+
+if __name__ == "__main__":
+  plant_thread()
+

selfdrive/test/plant/runtest.sh

@@ -0,0 +1,10 @@
+#!/bin/bash
+pushd ../../controls
+./controlsd.py &
+pid1=$!
+./radard.py &
+pid2=$!
+trap "trap - SIGTERM && kill $pid1 && kill $pid2" SIGINT SIGTERM EXIT
+popd
+mkdir -p out
+MPLBACKEND=svg ./runtracks.py out

selfdrive/test/plant/runtracks.py

@@ -0,0 +1,207 @@
+#!/usr/bin/env python
+import sys
+import time, json
+
+from selfdrive.test.plant import plant
+from selfdrive.config import Conversions as CV, CruiseButtons as CB
+from maneuver import *
+
+maneuvers = [
+  Maneuver(
+    'while cruising at 40 mph, change cruise speed to 50mph', 
+    duration=30.,
+    initial_speed = 40. * CV.MPH_TO_MS,
+    cruise_button_presses = [(CB.DECEL_SET, 2.), (0, 2.3),
+                             (CB.RES_ACCEL, 10.), (0, 10.1),
+                             (CB.RES_ACCEL, 10.2), (0, 10.3)]
+  ),
+  Maneuver(
+    'while cruising at 60 mph, change cruise speed to 50mph', 
+    duration=30., 
+    initial_speed=60. * CV.MPH_TO_MS,
+    cruise_button_presses = [(CB.DECEL_SET, 2.), (0, 2.3),
+                             (CB.DECEL_SET, 10.), (0, 10.1),
+                             (CB.DECEL_SET, 10.2), (0, 10.3)]
+  ),
+  Maneuver(
+    'while cruising at 20mph, grade change +10%', 
+    duration=25.,
+    initial_speed=20. * CV.MPH_TO_MS, 
+    cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)],
+    grade_values = [0., 0., 1.0],
+    grade_breakpoints = [0., 10., 11.]
+  ),
+  Maneuver(
+    'while cruising at 20mph, grade change -10%', 
+    duration=25.,
+    initial_speed=20. * CV.MPH_TO_MS, 
+    cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)],
+    grade_values = [0., 0., -1.0],
+    grade_breakpoints = [0., 10., 11.]
+  ),
+  Maneuver(
+    'approaching a 40mph car while cruising at 60mph from 100m away', 
+    duration=30., 
+    initial_speed = 60. * CV.MPH_TO_MS, 
+    lead_relevancy=True, 
+    initial_distance_lead=100., 
+    speed_lead_values = [40.*CV.MPH_TO_MS, 40.*CV.MPH_TO_MS],
+    speed_lead_breakpoints = [0., 100.],
+    cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)]
+  ),
+  Maneuver(
+    'approaching a 0mph car while cruising at 40mph from 150m away', 
+    duration=30., 
+    initial_speed = 40. * CV.MPH_TO_MS, 
+    lead_relevancy=True, 
+    initial_distance_lead=150., 
+    speed_lead_values = [0.*CV.MPH_TO_MS, 0.*CV.MPH_TO_MS],
+    speed_lead_breakpoints = [0., 100.],
+    cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)]
+  ),
+  Maneuver(
+    'steady state following a car at 20m/s, then lead decel to 0mph at 1m/s^2', 
+    duration=50., 
+    initial_speed = 20., 
+    lead_relevancy=True, 
+    initial_distance_lead=35., 
+    speed_lead_values = [20.*CV.MPH_TO_MS, 20.*CV.MPH_TO_MS, 0.*CV.MPH_TO_MS],
+    speed_lead_breakpoints = [0., 15., 35.0],
+    cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)]
+  ),
+  Maneuver(
+    'steady state following a car at 20m/s, then lead decel to 0mph at 2m/s^2', 
+    duration=50., 
+    initial_speed = 20., 
+    lead_relevancy=True, 
+    initial_distance_lead=35., 
+    speed_lead_values = [20.*CV.MPH_TO_MS, 20.*CV.MPH_TO_MS, 0.*CV.MPH_TO_MS],
+    speed_lead_breakpoints = [0., 15., 25.0],
+    cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)]
+  ),
+  Maneuver(
+    'starting at 0mph, approaching a stopped car 100m away', 
+    duration=30., 
+    initial_speed = 0., 
+    lead_relevancy=True, 
+    initial_distance_lead=100., 
+    cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3),
+                             (CB.RES_ACCEL, 1.4), (0.0, 1.5), 
+                             (CB.RES_ACCEL, 1.6), (0.0, 1.7), 
+                             (CB.RES_ACCEL, 1.8), (0.0, 1.9)]
+  ),
+  Maneuver(
+    "following a car at 60mph, lead accel and decel at 0.5m/s^2 every 2s",
+    duration=25.,
+    initial_speed=30.,
+    lead_relevancy=True,
+    initial_distance_lead=49.,
+    speed_lead_values=[30.,30.,29.,31.,29.,31.,29.],
+    speed_lead_breakpoints=[0., 6., 8., 12.,16.,20.,24.],
+    cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3),
+                             (CB.RES_ACCEL, 1.4), (0.0, 1.5), 
+                             (CB.RES_ACCEL, 1.6), (0.0, 1.7)]
+  ),
+  Maneuver(
+    "following a car at 10mph, stop and go at 1m/s2 lead dece1 and accel",
+    duration=70.,
+    initial_speed=10.,
+    lead_relevancy=True,
+    initial_distance_lead=20.,
+    speed_lead_values=[10., 0., 0., 10., 0.,10.],
+    speed_lead_breakpoints=[10., 20., 30., 40., 50., 60.],
+    cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3),
+                             (CB.RES_ACCEL, 1.4), (0.0, 1.5), 
+                             (CB.RES_ACCEL, 1.6), (0.0, 1.7)]
+  ),
+  Maneuver(
+    "green light: stopped behind lead car, lead car accelerates at 1.5 m/s",
+    duration=30.,
+    initial_speed=0.,
+    lead_relevancy=True,
+    initial_distance_lead=4.,
+    speed_lead_values=[0,  0 , 45],
+    speed_lead_breakpoints=[0, 10., 40.],
+    cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3),
+                             (CB.RES_ACCEL, 1.4), (0.0, 1.5), 
+                             (CB.RES_ACCEL, 1.6), (0.0, 1.7), 
+                             (CB.RES_ACCEL, 1.8), (0.0, 1.9), 
+                             (CB.RES_ACCEL, 2.0), (0.0, 2.1), 
+                             (CB.RES_ACCEL, 2.2), (0.0, 2.3)]
+  ),
+  Maneuver(
+    "stop and go with 1m/s2 lead decel and accel, with full stops",
+    duration=70.,
+    initial_speed=0.,
+    lead_relevancy=True,
+    initial_distance_lead=20.,
+    speed_lead_values=[10., 0., 0., 10., 0., 0.] ,
+    speed_lead_breakpoints=[10., 20., 30., 40., 50., 60.],
+    cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3),
+                             (CB.RES_ACCEL, 1.4), (0.0, 1.5), 
+                             (CB.RES_ACCEL, 1.6), (0.0, 1.7)] 
+  ),
+  Maneuver(
+    "accelerate from 20 while lead vehicle decelerates from 40 to 20 at 1m/s2",
+    duration=30.,
+    initial_speed=10.,
+    lead_relevancy=True,
+    initial_distance_lead=10.,
+    speed_lead_values=[20., 10.],
+    speed_lead_breakpoints=[1., 11.],
+    cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3),
+                             (CB.RES_ACCEL, 1.4), (0.0, 1.5), 
+                             (CB.RES_ACCEL, 1.6), (0.0, 1.7), 
+                             (CB.RES_ACCEL, 1.8), (0.0, 1.9), 
+                             (CB.RES_ACCEL, 2.0), (0.0, 2.1), 
+                             (CB.RES_ACCEL, 2.2), (0.0, 2.3)]
+  ),
+  Maneuver(
+    "accelerate from 20 while lead vehicle decelerates from 40 to 0 at 2m/s2",
+    duration=30.,
+    initial_speed=10.,
+    lead_relevancy=True,
+    initial_distance_lead=10.,
+    speed_lead_values=[20., 0.],
+    speed_lead_breakpoints=[1., 11.],
+    cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3),
+                             (CB.RES_ACCEL, 1.4), (0.0, 1.5), 
+                             (CB.RES_ACCEL, 1.6), (0.0, 1.7), 
+                             (CB.RES_ACCEL, 1.8), (0.0, 1.9), 
+                             (CB.RES_ACCEL, 2.0), (0.0, 2.1), 
+                             (CB.RES_ACCEL, 2.2), (0.0, 2.3)]
+  )
+]
+
+css_style = """
+.maneuver_title {
+  font-size: 24px;
+  text-align: center;
+}
+.maneuver_graph {
+  width: 100%;
+}
+"""
+
+def main(output_dir):
+  view_html = "<html><head><style>%s</style></head><body><table>" % (css_style,)
+  for i, man in enumerate(maneuvers):
+    view_html += "<tr><td class='maneuver_title' colspan=5><div>%s</div></td></tr><tr>" % (man.title,)
+    for c in ['distance.svg', 'speeds.svg', 'acceleration.svg', 'pedals.svg', 'pid.svg']:
+      view_html += "<td><img class='maneuver_graph' src='%s'/></td>" % (os.path.join("maneuver" + str(i+1).zfill(2), c), )
+    view_html += "</tr>"
+
+  with open(os.path.join(output_dir, "index.html"), "w") as f:
+    f.write(view_html)
+
+  for i, man in enumerate(maneuvers):
+    score, plot = man.evaluate()
+    plot.write_plot(output_dir, "maneuver" + str(i+1).zfill(2))
+
+if __name__ == "__main__":
+  if len(sys.argv) <= 1:
+    print "Usage:", sys.argv[0], "<output_dir>"
+    exit(1)
+
+  main(sys.argv[1])
+

selfdrive/ui/ui.c

@@ -133,10 +133,31 @@ typedef struct UIState {
   GLuint frame_front_tex;
 
   UIScene scene;
-
-
+  
+  bool awake;
+  int awake_timeout;
 } UIState;
 
+static void set_awake(UIState *s, bool awake) {
+  if (awake) {
+    // 30 second timeout
+    s->awake_timeout = 30;
+  }
+  if (s->awake != awake) {
+    s->awake = awake;
+
+    // TODO: actually turn off the screen and not just the backlight
+    FILE *f = fopen("/sys/class/leds/lcd-backlight/brightness", "wb");
+    if (f != NULL) {
+      if (awake) {
+        fprintf(f, "205");
+      } else {
+        fprintf(f, "0");
+      }
+      fclose(f);
+    }
+  }
+}
 
 static bool activity_running() {
   return system("dumpsys activity activities | grep mFocusedActivity > /dev/null") == 0;
@@ -173,13 +194,13 @@ typedef struct Button {
 static const Button buttons[] = {
   {
     .label = "wifi",
-    .x = 400, .y = 700, .w = 250, .h = 250,
+    .x = 400, .y = 730, .w = 250, .h = 250,
     .pressed = wifi_pressed,
     .enabled = wifi_enabled,
   },
   {
     .label = "ap",
-    .x = 1300, .y = 700, .w = 250, .h = 250,
+    .x = 1300, .y = 730, .w = 250, .h = 250,
     .pressed = ap_pressed,
     .enabled = ap_enabled,
   }
@@ -380,6 +401,9 @@ static void ui_init(UIState *s) {
   glDisable(GL_DEPTH_TEST);
 
   assert(glGetError() == GL_NO_ERROR);
+
+  // set awake
+  set_awake(s, true);
 }
 
 
@@ -1123,7 +1147,7 @@ static void ui_update(UIState *s) {
         s->scene.v_cruise = datad.vCruise;
         s->scene.v_ego = datad.vEgo;
         s->scene.angle_steers = datad.angleSteers;
-        s->scene.engaged = (datad.hudLead == 1) || (datad.hudLead == 2);
+        s->scene.engaged = datad.enabled;
         // printf("recv %f\n", datad.vEgo);
 
         s->scene.frontview = datad.rearViewCam;
@@ -1197,6 +1221,12 @@ static void ui_update(UIState *s) {
     if (rx_bytes) free(rx_bytes);
     if (tx_bytes) free(tx_bytes);
 
+    // TODO: do this properly
+    system("git rev-parse --abbrev-ref HEAD > /tmp/git_branch");
+    char *git_branch = read_file("/tmp/git_branch");
+    system("git rev-parse --short HEAD > /tmp/git_commit");
+    char *git_commit = read_file("/tmp/git_commit");
+
     int pending = pending_uploads();
 
     // service call wifi 20  # getWifiEnabledState
@@ -1210,14 +1240,29 @@ static void ui_update(UIState *s) {
     s->board_connected = !system("lsusb | grep bbaa > /dev/null");
 
     snprintf(s->base_text, sizeof(s->base_text),
-             "serial: %s\n dongle id: %s\n battery: %s %s\npending: %d\nrx %.1fkiB/s tx %.1fkiB/s\nboard: %s",
+             "version: %s (%s)\nserial: %s\n dongle id: %s\n battery: %s %s\npending: %d\nrx %.1fkiB/s tx %.1fkiB/s\nboard: %s",
+             git_commit, git_branch,
              s->serial, s->dongle_id, bat_cap ? bat_cap : "(null)", bat_stat ? bat_stat : "(null)",
              pending, rx_rate / 1024.0, tx_rate / 1024.0, s->board_connected ? "found" : "NOT FOUND");
 
     if (bat_cap) free(bat_cap);
     if (bat_stat) free(bat_stat);
 
+    if (git_branch) free(git_branch);
+    if (git_commit) free(git_commit);
+
     s->last_base_update = ts;
+
+    if (s->awake_timeout > 0) {
+      s->awake_timeout--;
+    } else {
+      set_awake(s, false);
+    }
+  }
+
+  if (s->vision_connected) {
+    // always awake if vision is connected
+    set_awake(s, true);
   }
 
   if (!s->vision_connected) {
@@ -1226,16 +1271,20 @@ static void ui_update(UIState *s) {
     int touch_x = -1, touch_y = -1;
     err = touch_poll(&s->touch, &touch_x, &touch_y);
     if (err == 1) {
-      // press buttons
-      for (int i=0; i<ARRAYSIZE(buttons); i++) {
-        const Button *b = &buttons[i];
-        if (touch_x >= b->x && touch_x < b->x+b->w
-            && touch_y >= b->y && touch_y < b->y+b->h) {
-          if (b->pressed && !activity_running()) {
-            b->pressed();
-            break;
+      if (s->awake) {
+        // press buttons
+        for (int i=0; i<ARRAYSIZE(buttons); i++) {
+          const Button *b = &buttons[i];
+          if (touch_x >= b->x && touch_x < b->x+b->w
+              && touch_y >= b->y && touch_y < b->y+b->h) {
+            if (b->pressed && !activity_running()) {
+              b->pressed();
+              break;
+            }
           }
         }
+      } else {
+        set_awake(s, true);
       }
     }
   }