openpilot v0.2 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

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,19 @@
+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/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

@@ -27,6 +27,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 +149,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 +183,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;
@@ -270,6 +276,11 @@ int main() {
   err = setpriority(PRIO_PROCESS, 0, -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,19 +18,23 @@ 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=[]):
   ret = []
-  for msg in dat.can:
+  for msg in dat:
     if msg.src in src_filter:
       ret.append([msg.address, msg.busTime, msg.dat.encode("hex")])
   return ret

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
 
@@ -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

@@ -2,11 +2,11 @@ import numpy as np
 
 import selfdrive.messaging as messaging
 from selfdrive.boardd.boardd import can_capnp_to_can_list_old, can_capnp_to_can_list
-from selfdrive.controls.lib.can_parser import CANParser
-from selfdrive.controls.lib.fingerprints import fingerprints
 from selfdrive.config import VehicleParams
 from common.realtime import sec_since_boot
 
+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),
@@ -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,233 @@
+#!/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_old
+
+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
+
+    # *** 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_old(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:
+      errors.append('commIssue')
+    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,85 +1,75 @@
 #!/usr/bin/env python
+import os
 import zmq
 import numpy as np
+import selfdrive.messaging as messaging
 
+from cereal import car
+
+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)
 
@@ -88,14 +78,54 @@ def controlsd_thread(gctx, rate=100):  #rate in Hz
     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)
+
+    # 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 = np.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 +135,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(np.maximum(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 +193,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 +263,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 +278,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 +313,4 @@ def main(gctx=None):
 
 if __name__ == "__main__":
   main()
+

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/latcontrol.py

@@ -97,24 +97,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)
     return final_steer, sat_flag

selfdrive/controls/lib/longcontrol.py

@@ -107,7 +107,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
@@ -151,9 +151,9 @@ 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):
+  def update(self, enabled, v_ego, v_cruise, v_target_lead, a_target, jerk_factor, VP):
     # TODO: not every time
-    if CS.brake_only:
+    if VP.brake_only:
       gas_max_v = [0, 0]                # values
     else:
       gas_max_v = [0.6, 0.6]            # values
@@ -162,8 +162,8 @@ class LongControl(object):
     brake_max_bp = [0., 5., 20., 100.]  # speeds     
 
     # brake and gas limits
-    brake_max = np.interp(CS.v_ego, brake_max_bp, brake_max_v)
-    gas_max = np.interp(CS.v_ego, gas_max_bp, gas_max_v)
+    brake_max = np.interp(v_ego, brake_max_bp, brake_max_v)
+    gas_max = np.interp(v_ego, gas_max_bp, gas_max_v)
 
     overshoot_allowance = 2.0    # overshoot allowed when changing accel sign
 
@@ -172,30 +172,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 = np.minimum(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 = np.maximum(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 = np.minimum(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 +206,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 = np.interp(v_ego, max_speed_error_bp, max_speed_error_v)
+        self.v_pid = np.minimum(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
+      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.)
     return final_gas, final_brake
+

selfdrive/controls/radard.py

@@ -1,4 +1,5 @@
 #!/usr/bin/env python
+import os
 import zmq
 import numpy as np
 import numpy.matlib
@@ -9,7 +10,6 @@ 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 +18,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):
@@ -41,30 +47,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 +55,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 +66,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 +81,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 +92,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 +150,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 +212,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,6 +280,8 @@ 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("NOBOARD") is not None:
     del managed_processes['boardd']
 

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_old
+
+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_old(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,139 @@
+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,275 @@
+#!/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_old, 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 += can_capnp_to_can_list_old(a.sendcan, [0,2])
+    #print can_msgs
+    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);
       }
     }
   }