openpilot v0.6.4 release

.gitignore

@@ -39,5 +39,6 @@ selfdrive/sensord/sensord
 
 one
 openpilot
+notebooks
 xx
 

README.md

@@ -58,8 +58,8 @@ Install openpilot on a neo device by entering ``https://openpilot.comma.ai`` dur
 Supported Cars
 ------
 
-| Make                 | Model                    | Supported Package    | Lateral | Longitudinal   | No Accel Below   | No Steer Below | Giraffe           |
-| ---------------------| -------------------------| ---------------------| --------| ---------------| -----------------| ---------------|-------------------|
+| Make                 | Model (US Market Reference)| Supported Package    | Lateral | Longitudinal   | No Accel Below   | No Steer Below | Giraffe           |
+| ---------------------| ---------------------------| ---------------------| --------| ---------------| -----------------| ---------------|-------------------|
 | Acura                | ILX 2016-18                | AcuraWatch Plus      | Yes     | Yes            | 25mph<sup>1</sup>| 25mph          | Nidec             |
 | Acura                | RDX 2016-18                | AcuraWatch Plus      | Yes     | Yes            | 25mph<sup>1</sup>| 12mph          | Nidec             |
 | Buick<sup>3</sup>    | Regal 2018                 | Adaptive Cruise      | Yes     | Yes            | 0mph             | 7mph           | Custom<sup>7</sup>|
@@ -79,7 +79,6 @@ Supported Cars
 | Honda                | CR-V 2015-16               | Touring              | Yes     | Yes            | 25mph<sup>1</sup>| 12mph          | Nidec             |
 | Honda                | CR-V 2017-19               | Honda Sensing        | Yes     | Stock          | 0mph             | 12mph          | Bosch             |
 | Honda                | CR-V Hybrid 2017-2019      | Honda Sensing        | Yes     | Stock          | 0mph             | 12mph          | Bosch             |
-| Honda                | Fit 2018                 | Honda Sensing        | Yes     | Yes            | 25mph<sup>1</sup>| 12mph          | Inverted Nidec    |
 | Honda                | Odyssey 2018-19            | Honda Sensing        | Yes     | Yes            | 25mph<sup>1</sup>| 0mph           | Inverted Nidec    |
 | Honda                | Passport 2019              | All                  | Yes     | Yes            | 25mph<sup>1</sup>| 12mph          | Inverted Nidec    |
 | Honda                | Pilot 2016-18              | Honda Sensing        | Yes     | Yes            | 25mph<sup>1</sup>| 12mph          | Nidec             |
@@ -95,6 +94,8 @@ Supported Cars
 | Kia                  | Stinger 2018               | SCC + LKAS           | Yes     | Stock          | 0mph             | 0mph           | Custom<sup>6</sup>|
 | Lexus                | ES Hybrid 2019             | All                  | Yes     | Yes            | 0mph             | 0mph           | Toyota            |
 | Lexus                | RX Hybrid 2016-19          | All                  | Yes     | Yes<sup>2</sup>| 0mph             | 0mph           | Toyota            |
+| Lexus                | IS 2017-2019               | All                  | Yes     | Stock          | 22mph            | 0mph           | Toyota            |
+| Lexus                | IS Hybrid 2017             | All                  | Yes     | Stock          | 0mph             | 0mph           | Toyota            |
 | Subaru               | Crosstrek 2018             | EyeSight             | Yes     | Stock          | 0mph             | 0mph           | Custom<sup>4</sup>|
 | Subaru               | Impreza 2019               | EyeSight             | Yes     | Stock          | 0mph             | 0mph           | Custom<sup>4</sup>|
 | Toyota               | Avalon 2016                | TSS-P                | Yes     | Yes<sup>2</sup>| 20mph<sup>1</sup>| 0mph           | Toyota            |
@@ -119,8 +120,8 @@ Supported Cars
 | Toyota               | Sienna 2018                | All                  | Yes     | Yes<sup>2</sup>| 0mph             | 0mph           | Toyota            |
 
 <sup>1</sup>[Comma Pedal](https://community.comma.ai/wiki/index.php/Comma_Pedal) is used to provide stop-and-go capability to some of the openpilot-supported cars that don't currently support stop-and-go. Here is how to [build a Comma Pedal](https://medium.com/@jfrux/comma-pedal-building-with-macrofab-6328bea791e8). ***NOTE: The Comma Pedal is not officially supported by [comma.ai](https://comma.ai).*** <br />
-<sup>2</sup>When disconnecting the Driver Support Unit (DSU), otherwise longitudinal control is stock ACC. For DSU locations, see [Toyota Wiki page](https://community.comma.ai/wiki/index.php/Toyota). <br />
-<sup>3</sup>[GM installation guide](https://zoneos.com/volt/). <br />
+<sup>2</sup>When disconnecting the Driver Support Unit (DSU), otherwise longitudinal control is stock ACC. For DSU locations, see [Toyota Wiki page](https://community.comma.ai/wiki/index.php/Toyota). ***NOTE: disconnecting the DSU disables Automatic Emergency Braking (AEB).*** <br />
+<sup>3</sup>[GM installation guide](https://zoneos.com/volt/). ***NOTE: disconnecting the ASCM disables Automatic Emergency Braking (AEB).*** <br />
 <sup>4</sup>Subaru Giraffe is DIY. <br />
 <sup>5</sup>28mph for Camry 4CYL L, 4CYL LE and 4CYL SE which don't have Full-Speed Range Dynamic Radar Cruise Control. <br />
 <sup>6</sup>Open sourced [Hyundai Giraffe](https://github.com/commaai/neo/tree/master/giraffe/hyundai) is designed for the 2019 Sante Fe; pinout may differ for other Hyundais. <br />
@@ -144,6 +145,7 @@ In Progress Cars
 - All Hyundai with SmartSense.
 - All Kia with SCC and LKAS.
 - All Chrysler, Jeep, Fiat with Adaptive Cruise Control and LaneSense.
+- All Subaru with EyeSight.
 
 How can I add support for my car?
 ------

RELEASES.md

@@ -1,3 +1,13 @@
+Version 0.6.4 (2019-09-08)
+========================
+ * Forward stock AEB for Honda Nidec
+ * Improve lane centering on banked roads
+ * Always-on forward collision warning
+ * Always-on driver monitoring, except for right hand drive countries
+ * Driver monitoring learns the user's normal driving position
+ * Honda Fit support thanks to energee!
+ * Lexus IS support
+
 Version 0.6.3 (2019-08-12)
 ========================
  * Alert sounds from EON: requires NEOS update

common/api/__init__.py

@@ -5,9 +5,10 @@ from datetime import datetime, timedelta
 from selfdrive.version import version
 
 class Api(object):
-  def __init__(self, dongle_id, private_key):
+  def __init__(self, dongle_id):
     self.dongle_id = dongle_id
-    self.private_key = private_key
+    with open('/persist/comma/id_rsa') as f:
+      self.private_key = f.read()
 
   def get(self, *args, **kwargs):
     return self.request('GET', *args, **kwargs)
@@ -19,7 +20,14 @@ class Api(object):
     return api_get(endpoint, method=method, timeout=timeout, access_token=access_token, **params)
 
   def get_token(self):
-    return jwt.encode({'identity': self.dongle_id, 'exp': datetime.utcnow() + timedelta(hours=1)}, self.private_key, algorithm='RS256')
+    now = datetime.utcnow()
+    payload = {
+      'identity': self.dongle_id,
+      'nbf': now,
+      'iat': now,
+      'exp': now + timedelta(hours=1)
+    }
+    return jwt.encode(payload, self.private_key, algorithm='RS256')
 
 def api_get(endpoint, method='GET', timeout=None, access_token=None, **params):
   backend = "https://api.commadotai.com/"

common/file_helpers.py

@@ -24,21 +24,12 @@ def rm_tree_or_link(path):
     shutil.rmtree(path)
 
 def get_tmpdir_on_same_filesystem(path):
-  # TODO(mgraczyk): HACK, we should actually check for which filesystem.
   normpath = os.path.normpath(path)
   parts = normpath.split("/")
-  if len(parts) > 1:
-    if parts[1].startswith("raid") or parts[1].startswith("datasets"):
-      if len(parts) > 2 and parts[2] == "runner":
-        return "/{}/runner/tmp".format(parts[1])
-      elif len(parts) > 2 and parts[2] == "aws":
-        return "/{}/aws/tmp".format(parts[1])
-      else:
-        return "/{}/tmp".format(parts[1])
-    elif parts[1] == "aws":
-      return "/aws/tmp"
-    elif parts[1] == "scratch":
+  if len(parts) > 1 and parts[1] == "scratch":
     return "/scratch/tmp"
+  elif len(parts) > 2 and parts[2] == "runner":
+    return "/{}/runner/tmp".format(parts[1])
   return "/tmp"
 
 class AutoMoveTempdir(object):

common/params.py

@@ -27,6 +27,7 @@ import sys
 import shutil
 import fcntl
 import tempfile
+import threading
 from enum import Enum
 
 
@@ -63,10 +64,9 @@ keys = {
   "GitCommit": [TxType.PERSISTENT],
   "GitRemote": [TxType.PERSISTENT],
   "HasAcceptedTerms": [TxType.PERSISTENT],
-  "IsDriverMonitoringEnabled": [TxType.PERSISTENT],
-  "IsFcwEnabled": [TxType.PERSISTENT],
   "IsGeofenceEnabled": [TxType.PERSISTENT],
   "IsMetric": [TxType.PERSISTENT],
+  "IsRHD": [TxType.PERSISTENT],
   "IsUpdateAvailable": [TxType.PERSISTENT],
   "IsUploadRawEnabled": [TxType.PERSISTENT],
   "IsUploadVideoOverCellularEnabled": [TxType.PERSISTENT],
@@ -346,6 +346,17 @@ class Params(object):
 
     write_db(self.db, key, dat)
 
+
+def put_nonblocking(key, val):
+  def f(key, val):
+    params = Params()
+    params.put(key, val)
+
+  t = threading.Thread(target=f, args=(key, val))
+  t.start()
+  return t
+
+
 if __name__ == "__main__":
   params = Params()
   if len(sys.argv) > 2:

common/stat_live.py

@@ -0,0 +1,73 @@
+import numpy as np
+
+class RunningStat():
+  # tracks realtime mean and standard deviation without storing any data
+  def __init__(self, priors=None, max_trackable=-1):
+    self.max_trackable = max_trackable
+    if priors is not None:
+      # initialize from history
+      self.M = priors[0]
+      self.S = priors[1]
+      self.n = priors[2]
+      self.M_last = self.M
+      self.S_last = self.S
+      
+    else:
+      self.reset()
+
+  def reset(self):
+    self.M = 0.
+    self.S = 0.
+    self.M_last = 0.
+    self.S_last = 0.
+    self.n = 0
+
+  def push_data(self, new_data):
+    # short term memory hack
+    if self.max_trackable < 0 or self.n < self.max_trackable:
+      self.n += 1
+    if self.n == 0:
+      self.M_last = new_data
+      self.M = self.M_last
+      self.S_last = 0.
+    else:
+      self.M = self.M_last + (new_data - self.M_last) / self.n
+      self.S = self.S_last + (new_data - self.M_last) * (new_data - self.M);
+      self.M_last = self.M
+      self.S_last = self.S
+
+  def mean(self):
+    return self.M
+
+  def variance(self):
+    if self.n >= 2:
+      return self.S / (self.n - 1.)
+    else:
+      return 0
+
+  def std(self):
+    return np.sqrt(self.variance())
+
+  def params_to_save(self):
+    return [self.M, self.S, self.n]
+
+class RunningStatFilter():
+  def __init__(self, raw_priors=None, filtered_priors=None, max_trackable=-1):
+    self.raw_stat = RunningStat(raw_priors, max_trackable)
+    self.filtered_stat = RunningStat(filtered_priors, max_trackable)
+
+  def reset(self):
+    self.raw_stat.reset()
+    self.filtered_stat.reset()
+
+  def push_and_update(self, new_data):
+    _std_last = self.raw_stat.std()
+    self.raw_stat.push_data(new_data)
+    _delta_std = self.raw_stat.std() - _std_last
+    if _delta_std<=0:
+      self.filtered_stat.push_data(new_data)
+    else:
+      pass
+      # self.filtered_stat.push_data(self.filtered_stat.mean())
+
+# class SequentialBayesian():

common/transformations/camera.py

@@ -154,17 +154,40 @@ def rotate_img(img, eulers, crop=None, intrinsics=eon_intrinsics):
                     W_border: size[1] - W_border]
 
 
+def get_camera_frame_from_calib_frame(camera_frame_from_road_frame):
+  camera_frame_from_ground = camera_frame_from_road_frame[:, (0, 1, 3)]
+  calib_frame_from_ground = np.dot(eon_intrinsics,
+                                     get_view_frame_from_road_frame(0, 0, 0, 1.22))[:, (0, 1, 3)]
+  ground_from_calib_frame = np.linalg.inv(calib_frame_from_ground)
+  camera_frame_from_calib_frame = np.dot(camera_frame_from_ground, ground_from_calib_frame)
+  return camera_frame_from_calib_frame
+
+
+def pretransform_from_calib(calib):
+  roll, pitch, yaw, height = calib
+  view_frame_from_road_frame = get_view_frame_from_road_frame(roll, pitch, yaw, height)
+  camera_frame_from_road_frame = np.dot(eon_intrinsics, view_frame_from_road_frame)
+  camera_frame_from_calib_frame = get_camera_frame_from_calib_frame(camera_frame_from_road_frame)
+  return np.linalg.inv(camera_frame_from_calib_frame)
+
+
 def transform_img(base_img,
                  augment_trans=np.array([0,0,0]),
                  augment_eulers=np.array([0,0,0]),
                  from_intr=eon_intrinsics,
                  to_intr=eon_intrinsics,
-                 calib_rot_view=None,
                  output_size=None,
                  pretransform=None,
-                 top_hacks=True):
+                 top_hacks=False,
+                 yuv=False,
+                 alpha=1.0,
+                 beta=0,
+                 blur=0):
   import cv2
 
+  if yuv:
+    base_img = cv2.cvtColor(base_img, cv2.COLOR_YUV2RGB_I420)
+
   size = base_img.shape[:2]
   if not output_size:
     output_size = size[::-1]
@@ -180,8 +203,6 @@ def transform_img(base_img,
                                         h*np.ones(4),
                                         h/quadrangle_norm[:,1]))
     rot = orient.rot_from_euler(augment_eulers)
-    if calib_rot_view is not None:
-      rot = calib_rot_view.dot(rot)
     to_extrinsics = np.hstack((rot.T, -augment_trans[:,None]))
     to_KE = to_intr.dot(to_extrinsics)
     warped_quadrangle_full = np.einsum('jk,ik->ij', to_KE, np.hstack((quadrangle_world, np.ones((4,1)))))
@@ -202,7 +223,19 @@ def transform_img(base_img,
       M = M.dot(pretransform)
     augmented_rgb[:cyy] = cv2.warpPerspective(base_img, M, (output_size[0], cyy), borderMode=cv2.BORDER_REPLICATE)
 
-  return augmented_rgb
+  # brightness and contrast augment
+  augmented_rgb = np.clip((float(alpha)*augmented_rgb + beta), 0, 255).astype(np.uint8)
+
+  # gaussian blur
+  if blur > 0:
+    augmented_rgb = cv2.GaussianBlur(augmented_rgb,(blur*2+1,blur*2+1),cv2.BORDER_DEFAULT)
+
+  if yuv:
+    augmented_img = cv2.cvtColor(augmented_rgb, cv2.COLOR_RGB2YUV_I420)
+  else:
+    augmented_img = augmented_rgb
+  return augmented_img
+
 
 def yuv_crop(frame, output_size, center=None):
   # output_size in camera coordinates so u,v

common/transformations/model.py

@@ -1,8 +1,8 @@
 import numpy as np
 
-from common.transformations.camera import eon_focal_length, \
-        vp_from_ke, get_view_frame_from_road_frame, \
-        FULL_FRAME_SIZE
+from common.transformations.camera import (FULL_FRAME_SIZE, eon_focal_length,
+                                           get_view_frame_from_road_frame,
+                                           vp_from_ke)
 
 # segnet
 

installer/updater/updater.cc

@@ -117,11 +117,19 @@ size_t download_file_write(void *ptr, size_t size, size_t nmeb, void *up) {
   return fwrite(ptr, size, nmeb, (FILE*)up);
 }
 
-bool check_battery() {
+int battery_capacity() {
   std::string bat_cap_s = util::read_file("/sys/class/power_supply/battery/capacity");
-  int bat_cap = atoi(bat_cap_s.c_str());
+  return atoi(bat_cap_s.c_str());
+}
+
+int battery_current() {
   std::string current_now_s = util::read_file("/sys/class/power_supply/battery/current_now");
-  int current_now = atoi(current_now_s.c_str());
+  return atoi(current_now_s.c_str());
+}
+
+bool check_battery() {
+  int bat_cap = battery_capacity();
+  int current_now = battery_current();
   return bat_cap > 35 || (current_now < 0 && bat_cap > 10);
 }
 
@@ -163,6 +171,7 @@ struct Updater {
   // i hate state machines give me coroutines already
   enum UpdateState {
     CONFIRMATION,
+    LOW_BATTERY,
     RUNNING,
     ERROR,
   };
@@ -173,6 +182,12 @@ struct Updater {
 
   std::string error_text;
 
+  std::string low_battery_text;
+  std::string low_battery_title;
+  std::string low_battery_context;
+  std::string battery_cap_text;
+  int min_battery_cap = 35;
+
   // button
   int b_x, b_w, b_y, b_h;
   int balt_x;
@@ -296,6 +311,16 @@ struct Updater {
     state = ERROR;
   }
 
+  void set_battery_low() {
+    std::lock_guard<std::mutex> guard(lock);
+    state = LOW_BATTERY;
+  }
+
+  void set_running() {
+    std::lock_guard<std::mutex> guard(lock);
+    state = RUNNING;
+  }
+
   std::string stage_download(std::string url, std::string hash, std::string name) {
     std::string out_fn = UPDATE_DIR "/" + util::base_name(url);
 
@@ -323,8 +348,14 @@ struct Updater {
     assert(curl);
 
     if (!check_battery()) {
-      set_error("Please plug power in to your EON and wait for charge");
-      return;
+      set_battery_low();
+      int battery_cap = battery_capacity();
+      while(battery_cap < min_battery_cap) {
+        battery_cap = battery_capacity();
+        battery_cap_text = std::to_string(battery_cap);
+        usleep(1000000);
+      }
+      set_running();
     }
 
     if (!check_space()) {
@@ -400,8 +431,14 @@ struct Updater {
     }
 
     if (!check_battery()) {
-      set_error("must have at least 35% battery to update");
-      return;
+      set_battery_low();
+      int battery_cap = battery_capacity();
+      while(battery_cap < min_battery_cap) {
+        battery_cap = battery_capacity();
+        battery_cap_text = std::to_string(battery_cap);
+        usleep(1000000);
+      }
+      set_running();
     }
 
     if (!recovery_fn.empty()) {
@@ -526,6 +563,27 @@ struct Updater {
     }
   }
 
+  void draw_battery_screen() {
+    low_battery_title = "Low Battery";
+    low_battery_text = "Please connect EON to your charger. Update will continue once EON battery reaches 35%.";
+    low_battery_context = "Current battery charge: " + battery_cap_text + "%";
+
+    nvgFillColor(vg, nvgRGBA(255,255,255,255));
+    nvgTextAlign(vg, NVG_ALIGN_CENTER | NVG_ALIGN_BASELINE);
+
+    nvgFontFace(vg, "opensans_bold");
+    nvgFontSize(vg, 120.0f);
+    nvgTextBox(vg, 110, 220, fb_w-240, low_battery_title.c_str(), NULL);
+
+    nvgFontFace(vg, "opensans_regular");
+    nvgFontSize(vg, 86.0f);
+    nvgTextBox(vg, 130, 380, fb_w-260, low_battery_text.c_str(), NULL);
+
+    nvgFontFace(vg, "opensans_bold");
+    nvgFontSize(vg, 86.0f);
+    nvgTextBox(vg, 130, 700, fb_w-260, low_battery_context.c_str(), NULL);
+  }
+
   void draw_progress_screen() {
     // draw progress message
     nvgFontSize(vg, 64.0f);
@@ -584,11 +642,14 @@ struct Updater {
                       "Continue",
                       "Connect to WiFi");
       break;
+    case LOW_BATTERY:
+      draw_battery_screen();
+      break;
     case RUNNING:
       draw_progress_screen();
       break;
     case ERROR:
-      draw_ack_screen("There was an error.", ("ERROR: " + error_text + "\n\nYou will need to retry").c_str(), NULL, "exit");
+      draw_ack_screen("There was an error", (error_text).c_str(), NULL, "Reboot");
       break;
     }
 
@@ -648,6 +709,7 @@ struct Updater {
       glDisable(GL_BLEND);
 
       eglSwapBuffers(display, surface);
+
       assert(glGetError() == GL_NO_ERROR);
 
       // no simple way to do 30fps vsync with surfaceflinger...

phonelibs/boringssl/LICENSE

@@ -0,0 +1,274 @@
+BoringSSL is a fork of OpenSSL. As such, large parts of it fall under OpenSSL
+licensing. Files that are completely new have a Google copyright and an ISC
+license. This license is reproduced at the bottom of this file.
+
+Contributors to BoringSSL are required to follow the CLA rules for Chromium:
+https://cla.developers.google.com/clas
+
+Files in third_party/ have their own licenses, as described therein. The MIT
+license, for third_party/fiat, which, unlike other third_party directories, is
+compiled into non-test libraries, is included below.
+
+The OpenSSL toolkit stays under a dual license, i.e. both the conditions of the
+OpenSSL License and the original SSLeay license apply to the toolkit. See below
+for the actual license texts. Actually both licenses are BSD-style Open Source
+licenses. In case of any license issues related to OpenSSL please contact
+openssl-core@openssl.org.
+
+The following are Google-internal bug numbers where explicit permission from
+some authors is recorded for use of their work. (This is purely for our own
+record keeping.)
+  27287199
+  27287880
+  27287883
+
+  OpenSSL License
+  ---------------
+
+/* ====================================================================
+ * Copyright (c) 1998-2011 The OpenSSL Project.  All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer. 
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ *
+ * 3. All advertising materials mentioning features or use of this
+ *    software must display the following acknowledgment:
+ *    "This product includes software developed by the OpenSSL Project
+ *    for use in the OpenSSL Toolkit. (http://www.openssl.org/)"
+ *
+ * 4. The names "OpenSSL Toolkit" and "OpenSSL Project" must not be used to
+ *    endorse or promote products derived from this software without
+ *    prior written permission. For written permission, please contact
+ *    openssl-core@openssl.org.
+ *
+ * 5. Products derived from this software may not be called "OpenSSL"
+ *    nor may "OpenSSL" appear in their names without prior written
+ *    permission of the OpenSSL Project.
+ *
+ * 6. Redistributions of any form whatsoever must retain the following
+ *    acknowledgment:
+ *    "This product includes software developed by the OpenSSL Project
+ *    for use in the OpenSSL Toolkit (http://www.openssl.org/)"
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE OpenSSL PROJECT ``AS IS'' AND ANY
+ * EXPRESSED OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+ * PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE OpenSSL PROJECT OR
+ * ITS CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
+ * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+ * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
+ * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED
+ * OF THE POSSIBILITY OF SUCH DAMAGE.
+ * ====================================================================
+ *
+ * This product includes cryptographic software written by Eric Young
+ * (eay@cryptsoft.com).  This product includes software written by Tim
+ * Hudson (tjh@cryptsoft.com).
+ *
+ */
+
+ Original SSLeay License
+ -----------------------
+
+/* Copyright (C) 1995-1998 Eric Young (eay@cryptsoft.com)
+ * All rights reserved.
+ *
+ * This package is an SSL implementation written
+ * by Eric Young (eay@cryptsoft.com).
+ * The implementation was written so as to conform with Netscapes SSL.
+ * 
+ * This library is free for commercial and non-commercial use as long as
+ * the following conditions are aheared to.  The following conditions
+ * apply to all code found in this distribution, be it the RC4, RSA,
+ * lhash, DES, etc., code; not just the SSL code.  The SSL documentation
+ * included with this distribution is covered by the same copyright terms
+ * except that the holder is Tim Hudson (tjh@cryptsoft.com).
+ * 
+ * Copyright remains Eric Young's, and as such any Copyright notices in
+ * the code are not to be removed.
+ * If this package is used in a product, Eric Young should be given attribution
+ * as the author of the parts of the library used.
+ * This can be in the form of a textual message at program startup or
+ * in documentation (online or textual) provided with the package.
+ * 
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ * 3. All advertising materials mentioning features or use of this software
+ *    must display the following acknowledgement:
+ *    "This product includes cryptographic software written by
+ *     Eric Young (eay@cryptsoft.com)"
+ *    The word 'cryptographic' can be left out if the rouines from the library
+ *    being used are not cryptographic related :-).
+ * 4. If you include any Windows specific code (or a derivative thereof) from 
+ *    the apps directory (application code) you must include an acknowledgement:
+ *    "This product includes software written by Tim Hudson (tjh@cryptsoft.com)"
+ * 
+ * THIS SOFTWARE IS PROVIDED BY ERIC YOUNG ``AS IS'' AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
+ * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+ * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
+ * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
+ * SUCH DAMAGE.
+ * 
+ * The licence and distribution terms for any publically available version or
+ * derivative of this code cannot be changed.  i.e. this code cannot simply be
+ * copied and put under another distribution licence
+ * [including the GNU Public Licence.]
+ */
+
+
+ISC license used for completely new code in BoringSSL:
+
+/* Copyright (c) 2015, Google Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY
+ * SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION
+ * OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN
+ * CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. */
+
+
+The code in third_party/fiat carries the MIT license:
+
+Copyright (c) 2015-2016 the fiat-crypto authors (see
+https://github.com/mit-plv/fiat-crypto/blob/master/AUTHORS).
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.
+
+
+The code in third_party/sike also carries the MIT license:
+
+Copyright (c) Microsoft Corporation. All rights reserved.
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE
+
+
+Licenses for support code
+-------------------------
+
+Parts of the TLS test suite are under the Go license. This code is not included
+in BoringSSL (i.e. libcrypto and libssl) when compiled, however, so
+distributing code linked against BoringSSL does not trigger this license:
+
+Copyright (c) 2009 The Go Authors. All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are
+met:
+
+   * Redistributions of source code must retain the above copyright
+notice, this list of conditions and the following disclaimer.
+   * Redistributions in binary form must reproduce the above
+copyright notice, this list of conditions and the following disclaimer
+in the documentation and/or other materials provided with the
+distribution.
+   * Neither the name of Google Inc. nor the names of its
+contributors may be used to endorse or promote products derived from
+this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+
+BoringSSL uses the Chromium test infrastructure to run a continuous build,
+trybots etc. The scripts which manage this, and the script for generating build
+metadata, are under the Chromium license. Distributing code linked against
+BoringSSL does not trigger this license.
+
+Copyright 2015 The Chromium Authors. All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are
+met:
+
+   * Redistributions of source code must retain the above copyright
+notice, this list of conditions and the following disclaimer.
+   * Redistributions in binary form must reproduce the above
+copyright notice, this list of conditions and the following disclaimer
+in the documentation and/or other materials provided with the
+distribution.
+   * Neither the name of Google Inc. nor the names of its
+contributors may be used to endorse or promote products derived from
+this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

phonelibs/bzip2/LICENSE

@@ -0,0 +1,42 @@
+
+--------------------------------------------------------------------------
+
+This program, "bzip2", the associated library "libbzip2", and all
+documentation, are copyright (C) 1996-2010 Julian R Seward.  All
+rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions
+are met:
+
+1. Redistributions of source code must retain the above copyright
+   notice, this list of conditions and the following disclaimer.
+
+2. The origin of this software must not be misrepresented; you must 
+   not claim that you wrote the original software.  If you use this 
+   software in a product, an acknowledgment in the product 
+   documentation would be appreciated but is not required.
+
+3. Altered source versions must be plainly marked as such, and must
+   not be misrepresented as being the original software.
+
+4. The name of the author may not be used to endorse or promote 
+   products derived from this software without specific prior written 
+   permission.
+
+THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS
+OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
+GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+Julian Seward, jseward@bzip.org
+bzip2/libbzip2 version 1.0.6 of 6 September 2010
+
+--------------------------------------------------------------------------

phonelibs/libyuv/LICENSE

@@ -0,0 +1,29 @@
+Copyright 2011 The LibYuv Project Authors. All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are
+met:
+
+  * Redistributions of source code must retain the above copyright
+    notice, this list of conditions and the following disclaimer.
+
+  * Redistributions in binary form must reproduce the above copyright
+    notice, this list of conditions and the following disclaimer in
+    the documentation and/or other materials provided with the
+    distribution.
+
+  * Neither the name of Google nor the names of its contributors may
+    be used to endorse or promote products derived from this software
+    without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

phonelibs/openblas/libopenblas.so

@@ -1 +0,0 @@
-libopenblas_armv8p-r0.2.19.so

phonelibs/yaml-cpp/LICENSE

@@ -0,0 +1,19 @@
+Copyright (c) 2008-2015 Jesse Beder.
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in
+all copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+THE SOFTWARE.

run_docker_tests.sh

@@ -13,4 +13,4 @@ docker run --rm tmppilot /bin/sh -c 'cd /tmp/openpilot/ && make -C cereal && pyt
 docker run --rm tmppilot /bin/sh -c 'cd /tmp/openpilot/ && python -m unittest discover selfdrive/loggerd'
 docker run --rm -v "$(pwd)"/selfdrive/test/tests/plant/out:/tmp/openpilot/selfdrive/test/tests/plant/out tmppilot /bin/sh -c 'cd /tmp/openpilot/selfdrive/test/tests/plant && OPTEST=1 ./test_longitudinal.py'
 docker run --rm tmppilot /bin/sh -c 'cd /tmp/openpilot/ && make -C cereal && cd /tmp/openpilot/selfdrive/test/tests/process_replay/ && ./test_processes.py'
-docker run --rm tmppilot /bin/sh -c 'mkdir -p /data/params && cd /tmp/openpilot/ && make -C cereal && cd /tmp/openpilot/selfdrive/test/ && ./test_car_models_openpilot.py'
+docker run --rm tmppilot /bin/sh -c 'mkdir -p /data/params && cd /tmp/openpilot/ && make -C cereal && cd /tmp/openpilot/selfdrive/test/ && ./test_car_models.py'

selfdrive/athena/athenad.py

@@ -1,6 +1,5 @@
 #!/usr/bin/env python2.7
 import json
-import jwt
 import os
 import random
 import re
@@ -13,7 +12,6 @@ import traceback
 import zmq
 import requests
 import six.moves.queue
-from datetime import datetime, timedelta
 from functools import partial
 from jsonrpc import JSONRPCResponseManager, dispatcher
 from websocket import create_connection, WebSocketTimeoutException, ABNF
@@ -104,9 +102,7 @@ def startLocalProxy(global_end_event, remote_ws_uri, local_port):
 
     params = Params()
     dongle_id = params.get("DongleId")
-    private_key = open("/persist/comma/id_rsa").read()
-    identity_token = jwt.encode({'identity':dongle_id, 'exp': datetime.utcnow() + timedelta(hours=1)}, private_key, algorithm='RS256')
-
+    identity_token = Api(dongle_id).get_token()
     ws = create_connection(remote_ws_uri,
                            cookie="jwt=" + identity_token,
                            enable_multithread=True)
@@ -232,8 +228,7 @@ def main(gctx=None):
   crash.bind_extra(version=version, dirty=dirty, is_eon=True)
   crash.install()
 
-  private_key = open("/persist/comma/id_rsa").read()
-  api = Api(dongle_id, private_key)
+  api = Api(dongle_id)
 
   conn_retries = 0
   while 1:

selfdrive/boardd/boardd.cc

@@ -43,6 +43,7 @@
 #define SAFETY_TESLA 8
 #define SAFETY_CHRYSLER 9
 #define SAFETY_SUBARU 10
+#define SAFETY_GM_PASSIVE 11
 #define SAFETY_TOYOTA_IPAS 0x1335
 #define SAFETY_TOYOTA_NOLIMITS 0x1336
 #define SAFETY_ALLOUTPUT 0x1337
@@ -135,6 +136,9 @@ void *safety_setter_thread(void *s) {
   case cereal::CarParams::SafetyModel::GM:
     safety_setting = SAFETY_GM;
     break;
+  case cereal::CarParams::SafetyModel::GM_PASSIVE:
+    safety_setting = SAFETY_GM_PASSIVE;
+    break;
   case cereal::CarParams::SafetyModel::HONDA_BOSCH:
     safety_setting = SAFETY_HONDA_BOSCH;
     break;

selfdrive/can/libdbc_py.py

@@ -82,7 +82,7 @@ void* can_init(int bus, const char* dbc_name,
 
 int can_update(void* can, uint64_t sec, bool wait);
 
-size_t can_query(void* can, uint64_t sec, bool *out_can_valid, size_t out_values_size, SignalValue* out_values);
+size_t can_query_latest(void* can, bool *out_can_valid, size_t out_values_size, SignalValue* out_values);
 
 const DBC* dbc_lookup(const char* dbc_name);
 

selfdrive/can/parser.cc

@@ -330,7 +330,7 @@ class CANParser {
     }
   }
 
-  void update_string(uint64_t sec, std::string data) {
+  void update_string(std::string data) {
     // format for board, make copy due to alignment issues, will be freed on out of scope
     auto amsg = kj::heapArray<capnp::word>((data.length() / sizeof(capnp::word)) + 1);
     memcpy(amsg.begin(), data.data(), data.length());
@@ -339,10 +339,12 @@ class CANParser {
     capnp::FlatArrayMessageReader cmsg(amsg);
     cereal::Event::Reader event = cmsg.getRoot<cereal::Event>();
 
+    last_sec = event.getLogMonoTime();
+
     auto cans = event.getCan();
-    UpdateCans(sec, cans);
+    UpdateCans(last_sec, cans);
 
-    UpdateValid(sec);
+    UpdateValid(last_sec);
   }
 
   int update(uint64_t sec, bool wait) {
@@ -381,17 +383,18 @@ class CANParser {
       UpdateCans(sec, cans);
     }
 
+    last_sec = sec;
     UpdateValid(sec);
     zmq_msg_close(&msg);
     return result;
   }
 
-  std::vector<SignalValue> query(uint64_t sec) {
+  std::vector<SignalValue> query_latest() {
     std::vector<SignalValue> ret;
 
     for (const auto& kv : message_states) {
       const auto& state = kv.second;
-      if (sec != 0 && state.seen != sec) continue;
+      if (last_sec != 0 && state.seen != last_sec) continue;
 
       for (int i=0; i<state.parse_sigs.size(); i++) {
         const Signal &sig = state.parse_sigs[i];
@@ -408,6 +411,7 @@ class CANParser {
   }
 
   bool can_valid = false;
+  uint64_t last_sec = 0;
 
  private:
   const int bus;
@@ -451,31 +455,31 @@ int can_update(void* can, uint64_t sec, bool wait) {
   return cp->update(sec, wait);
 }
 
-void can_update_string(void *can, uint64_t sec, const char* dat, int len) {
+void can_update_string(void *can, const char* dat, int len) {
   CANParser* cp = (CANParser*)can;
-  cp->update_string(sec, std::string(dat, len));
+  cp->update_string(std::string(dat, len));
 }
 
-size_t can_query(void* can, uint64_t sec, bool *out_can_valid, size_t out_values_size, SignalValue* out_values) {
+size_t can_query_latest(void* can, bool *out_can_valid, size_t out_values_size, SignalValue* out_values) {
   CANParser* cp = (CANParser*)can;
 
   if (out_can_valid) {
     *out_can_valid = cp->can_valid;
   }
 
-  const std::vector<SignalValue> values = cp->query(sec);
+  const std::vector<SignalValue> values = cp->query_latest();
   if (out_values) {
     std::copy(values.begin(), values.begin()+std::min(out_values_size, values.size()), out_values);
   }
   return values.size();
 };
 
-void can_query_vector(void* can, uint64_t sec, bool *out_can_valid, std::vector<SignalValue> &values) {
+void can_query_latest_vector(void* can, bool *out_can_valid, std::vector<SignalValue> &values) {
   CANParser* cp = (CANParser*)can;
   if (out_can_valid) {
     *out_can_valid = cp->can_valid;
   }
-  values = cp->query(sec);
+  values = cp->query_latest();
 };
 
 }

selfdrive/can/parser_pyx.pxd

@@ -67,9 +67,9 @@ ctypedef void* (*can_init_with_vectors_func)(int bus, const char* dbc_name,
                 const char* tcp_addr,
                 int timeout)
 ctypedef int (*can_update_func)(void* can, uint64_t sec, bool wait);
-ctypedef void (*can_update_string_func)(void* can, uint64_t sec, const char* dat, int len);
-ctypedef size_t (*can_query_func)(void* can, uint64_t sec, bool *out_can_valid, size_t out_values_size, SignalValue* out_values);
-ctypedef void (*can_query_vector_func)(void* can, uint64_t sec, bool *out_can_valid,  vector[SignalValue] &values)
+ctypedef void (*can_update_string_func)(void* can, const char* dat, int len);
+ctypedef size_t (*can_query_latest_func)(void* can, bool *out_can_valid, size_t out_values_size, SignalValue* out_values);
+ctypedef void (*can_query_latest_vector_func)(void* can, bool *out_can_valid,  vector[SignalValue] &values)
 
 cdef class CANParser:
   cdef:
@@ -79,7 +79,7 @@ cdef class CANParser:
     can_init_with_vectors_func can_init_with_vectors
     can_update_func can_update
     can_update_string_func can_update_string
-    can_query_vector_func can_query_vector
+    can_query_latest_vector_func can_query_latest_vector
     map[string, uint32_t] msg_name_to_address
     map[uint32_t, string] address_to_msg_name
     vector[SignalValue] can_values
@@ -91,4 +91,4 @@ cdef class CANParser:
     bool can_valid
     int can_invalid_cnt
 
-  cdef unordered_set[uint32_t] update_vl(self, uint64_t sec)
+  cdef unordered_set[uint32_t] update_vl(self)

selfdrive/can/parser_pyx.pyx

@@ -18,7 +18,7 @@ cdef class CANParser:
     self.dbc_lookup = <dbc_lookup_func>dlsym(libdbc, 'dbc_lookup')
     self.can_update = <can_update_func>dlsym(libdbc, 'can_update')
     self.can_update_string = <can_update_string_func>dlsym(libdbc, 'can_update_string')
-    self.can_query_vector = <can_query_vector_func>dlsym(libdbc, 'can_query_vector')
+    self.can_query_latest_vector = <can_query_latest_vector_func>dlsym(libdbc, 'can_query_latest_vector')
     if checks is None:
       checks = []
 
@@ -72,14 +72,14 @@ cdef class CANParser:
       message_options_v.push_back(mpo)
 
     self.can = self.can_init_with_vectors(bus, dbc_name, message_options_v, signal_options_v, sendcan, tcp_addr, timeout)
-    self.update_vl(0)
+    self.update_vl()
 
-  cdef unordered_set[uint32_t] update_vl(self, uint64_t sec):
+  cdef unordered_set[uint32_t] update_vl(self):
     cdef string sig_name
     cdef unordered_set[uint32_t] updated_val
     cdef bool valid = False
 
-    self.can_query_vector(self.can, sec, &valid, self.can_values)
+    self.can_query_latest_vector(self.can, &valid, self.can_values)
 
     # Update invalid flag
     self.can_invalid_cnt += 1
@@ -100,20 +100,20 @@ cdef class CANParser:
 
     return updated_val
 
-  def update_string(self, uint64_t sec, dat):
-    self.can_update_string(self.can, sec, dat, len(dat))
-    return self.update_vl(sec)
+  def update_string(self, dat):
+    self.can_update_string(self.can, dat, len(dat))
+    return self.update_vl()
 
-  def update_strings(self, uint64_t sec, strings):
+  def update_strings(self, strings):
     updated_vals = set()
 
     for s in strings:
-      updated_val = self.update_string(sec, s)
+      updated_val = self.update_string(s)
       updated_vals.update(updated_val)
 
     return updated_vals
 
   def update(self, uint64_t sec, bool wait):
     r = (self.can_update(self.can, sec, wait) >= 0)
-    updated_val = self.update_vl(sec)
+    updated_val = self.update_vl()
     return r, updated_val

selfdrive/can/tests/parser_old.py

@@ -71,12 +71,12 @@ class CANParser(object):
 
     self.p_can_valid = ffi.new("bool*")
 
-    value_count = libdbc.can_query(self.can, 0, self.p_can_valid, 0, ffi.NULL)
+    value_count = libdbc.can_query_latest(self.can, self.p_can_valid, 0, ffi.NULL)
     self.can_values = ffi.new("SignalValue[%d]" % value_count)
     self.update_vl(0)
 
   def update_vl(self, sec):
-    can_values_len = libdbc.can_query(self.can, sec, self.p_can_valid, len(self.can_values), self.can_values)
+    can_values_len = libdbc.can_query_latest(self.can, self.p_can_valid, len(self.can_values), self.can_values)
     assert can_values_len <= len(self.can_values)
 
     self.can_invalid_cnt += 1

selfdrive/can/tests/test_packer_toyota.py

@@ -47,32 +47,34 @@ class TestPackerMethods(unittest.TestCase):
       self.assertEqual(m_old, m)
 
       steer = (random.randint(0, 2) % 2 == 0)
+      chime = random.randint(1, 65536)
       left_line = (random.randint(0, 2) % 2 == 0)
       right_line = (random.randint(0, 2) % 2 == 0)
       left_lane_depart = (random.randint(0, 2) % 2 == 0)
       right_lane_depart = (random.randint(0, 2) % 2 == 0)
-      m_old = create_ui_command(self.cp_old, steer, left_line, right_line, left_lane_depart, right_lane_depart)
-      m = create_ui_command(self.cp, steer, left_line, right_line, left_lane_depart, right_lane_depart)
+      m_old = create_ui_command(self.cp_old, steer, chime, left_line, right_line, left_lane_depart, right_lane_depart)
+      m = create_ui_command(self.cp, steer, chime, left_line, right_line, left_lane_depart, right_lane_depart)
       self.assertEqual(m_old, m)
 
   def test_performance(self):
     n1 = sec_since_boot()
     recursions = 100000
     steer = (random.randint(0, 2) % 2 == 0)
+    chime = random.randint(1, 65536)
     left_line = (random.randint(0, 2) % 2 == 0)
     right_line = (random.randint(0, 2) % 2 == 0)
     left_lane_depart = (random.randint(0, 2) % 2 == 0)
     right_lane_depart = (random.randint(0, 2) % 2 == 0)
 
     for _ in xrange(recursions):
-      create_ui_command(self.cp_old, steer, left_line, right_line, left_lane_depart, right_lane_depart)
+      create_ui_command(self.cp_old, steer, chime, left_line, right_line, left_lane_depart, right_lane_depart)
     n2 = sec_since_boot()
     elapsed_old = n2 - n1
 
     # print('Old API, elapsed time: {} secs'.format(elapsed_old))
     n1 = sec_since_boot()
     for _ in xrange(recursions):
-      create_ui_command(self.cp, steer, left_line, right_line, left_lane_depart, right_lane_depart)
+      create_ui_command(self.cp, steer, chime, left_line, right_line, left_lane_depart, right_lane_depart)
     n2 = sec_since_boot()
     elapsed_new = n2 - n1
     # print('New API, elapsed time: {} secs'.format(elapsed_new))

selfdrive/can/tests/test_parser.py

@@ -58,16 +58,15 @@ def run_route(route):
 
   route_ok = True
 
-  t = 0
   for msg in lr:
     if msg.which() == 'can':
-      t += DT
+      t = msg.logMonoTime
       msg_bytes = msg.as_builder().to_bytes()
       can.send(msg_bytes)
 
       _, updated_old = parser_old.update(t, True)
       _, updated_new = parser_new.update(t, True)
-      updated_string = parser_string.update_string(t, msg_bytes)
+      updated_string = parser_string.update_string(msg_bytes)
 
       if updated_old != updated_new:
         route_ok = False

selfdrive/car/car_helpers.py

@@ -1,4 +1,5 @@
 import os
+import zmq
 from cereal import car
 from common.params import Params
 from common.vin import get_vin, VIN_UNKNOWN
@@ -8,6 +9,16 @@ from selfdrive.swaglog import cloudlog
 import selfdrive.messaging as messaging
 
 
+def get_one_can(logcan):
+  while True:
+    try:
+      can = messaging.recv_one(logcan)
+      if len(can.can) > 0:
+        return can
+    except zmq.error.Again:
+      continue
+
+
 def get_startup_alert(car_recognized, controller_available):
   alert = 'startup'
   if not car_recognized:
@@ -86,7 +97,7 @@ def fingerprint(logcan, sendcan, is_panda_black):
   done = False
 
   while not done:
-    a = messaging.recv_one(logcan)
+    a = get_one_can(logcan)
 
     for can in a.can:
       # need to independently try to fingerprint both bus 0 and 1 to work

selfdrive/car/chrysler/carcontroller.py

@@ -31,7 +31,7 @@ class CarController(object):
     self.packer = CANPacker(dbc_name)
 
 
-  def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, hud_alert):
+  def update(self, enabled, CS, actuators, pcm_cancel_cmd, hud_alert):
     # this seems needed to avoid steering faults and to force the sync with the EPS counter
     frame = CS.lkas_counter
     if self.prev_frame == frame:

selfdrive/car/chrysler/carstate.py

@@ -1,20 +1,22 @@
+from cereal import car
 from selfdrive.can.parser import CANParser
 from selfdrive.car.chrysler.values import DBC, STEER_THRESHOLD
 from common.kalman.simple_kalman import KF1D
 
+GearShifter = car.CarState.GearShifter
 
 def parse_gear_shifter(can_gear):
   if can_gear == 0x1:
-    return "park"
+    return GearShifter.park
   elif can_gear == 0x2:
-    return "reverse"
+    return GearShifter.reverse
   elif can_gear == 0x3:
-    return "neutral"
+    return GearShifter.neutral
   elif can_gear == 0x4:
-    return "drive"
+    return GearShifter.drive
   elif can_gear == 0x5:
-    return "low"
-  return "unknown"
+    return GearShifter.low
+  return GearShifter.unknown
 
 
 def get_can_parser(CP):

selfdrive/car/chrysler/interface.py

@@ -1,5 +1,4 @@
 #!/usr/bin/env python
-from common.realtime import sec_since_boot
 from cereal import car
 from selfdrive.config import Conversions as CV
 from selfdrive.controls.lib.drive_helpers import EventTypes as ET, create_event
@@ -8,13 +7,14 @@ from selfdrive.car.chrysler.carstate import CarState, get_can_parser, get_camera
 from selfdrive.car.chrysler.values import ECU, check_ecu_msgs, CAR
 from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness
 
+GearShifter = car.CarState.GearShifter
+ButtonType = car.CarState.ButtonEvent.Type
 
 class CarInterface(object):
   def __init__(self, CP, CarController):
     self.CP = CP
     self.VM = VehicleModel(CP)
 
-    self.frame = 0
     self.gas_pressed_prev = False
     self.brake_pressed_prev = False
     self.cruise_enabled_prev = False
@@ -115,8 +115,8 @@ class CarInterface(object):
   # returns a car.CarState
   def update(self, c, can_strings):
     # ******************* do can recv *******************
-    self.cp.update_strings(int(sec_since_boot() * 1e9), can_strings)
-    self.cp_cam.update_strings(int(sec_since_boot() * 1e9), can_strings)
+    self.cp.update_strings(can_strings)
+    self.cp_cam.update_strings(can_strings)
 
     self.CS.update(self.cp, self.cp_cam)
 
@@ -160,8 +160,6 @@ class CarInterface(object):
     ret.cruiseState.speed = self.CS.v_cruise_pcm * CV.KPH_TO_MS
     ret.cruiseState.available = self.CS.main_on
     ret.cruiseState.speedOffset = 0.
-    # ignore standstill in hybrid rav4, since pcm allows to restart without
-    # receiving any special command
     ret.cruiseState.standstill = False
 
     # TODO: button presses
@@ -169,13 +167,13 @@ class CarInterface(object):
 
     if self.CS.left_blinker_on != self.CS.prev_left_blinker_on:
       be = car.CarState.ButtonEvent.new_message()
-      be.type = 'leftBlinker'
+      be.type = ButtonType.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.type = ButtonType.rightBlinker
       be.pressed = self.CS.right_blinker_on != 0
       buttonEvents.append(be)
 
@@ -193,7 +191,7 @@ class CarInterface(object):
 
     # events
     events = []
-    if not (ret.gearShifter in ('drive', 'low')):
+    if not (ret.gearShifter in (GearShifter.drive, GearShifter.low)):
       events.append(create_event('wrongGear', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
     if ret.doorOpen:
       events.append(create_event('doorOpen', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
@@ -203,7 +201,7 @@ class CarInterface(object):
       events.append(create_event('espDisabled', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
     if not self.CS.main_on:
       events.append(create_event('wrongCarMode', [ET.NO_ENTRY, ET.USER_DISABLE]))
-    if ret.gearShifter == 'reverse':
+    if ret.gearShifter == GearShifter.reverse:
       events.append(create_event('reverseGear', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
     if self.CS.steer_error:
       events.append(create_event('steerUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT]))
@@ -236,8 +234,6 @@ class CarInterface(object):
     if (self.CS.frame == -1):
       return [] # if we haven't seen a frame 220, then do not update.
 
-    self.frame = self.CS.frame
-    can_sends = self.CC.update(c.enabled, self.CS, self.frame,
-                               c.actuators, c.cruiseControl.cancel, c.hudControl.visualAlert)
+    can_sends = self.CC.update(c.enabled, self.CS, c.actuators, c.cruiseControl.cancel, c.hudControl.visualAlert)
 
     return can_sends

selfdrive/car/chrysler/radar_interface.py

@@ -2,7 +2,6 @@
 import os
 from selfdrive.can.parser import CANParser
 from cereal import car
-from common.realtime import sec_since_boot
 
 RADAR_MSGS_C = range(0x2c2, 0x2d4+2, 2)  # c_ messages 706,...,724
 RADAR_MSGS_D = range(0x2a2, 0x2b4+2, 2)  # d_ messages
@@ -55,8 +54,7 @@ class RadarInterface(object):
     self.trigger_msg = LAST_MSG
 
   def update(self, can_strings):
-    tm = int(sec_since_boot() * 1e9)
-    vls = self.rcp.update_strings(tm, can_strings)
+    vls = self.rcp.update_strings(can_strings)
     self.updated_messages.update(vls)
 
     if self.trigger_msg not in self.updated_messages:

selfdrive/car/ford/carcontroller.py

@@ -0,0 +1,87 @@
+from cereal import car
+from selfdrive.car.ford.fordcan import make_can_msg, create_steer_command, create_lkas_ui, \
+                                       spam_cancel_button
+from selfdrive.can.packer import CANPacker
+
+
+MAX_STEER_DELTA = 1
+TOGGLE_DEBUG = False
+
+class CarController(object):
+  def __init__(self, dbc_name, enable_camera, vehicle_model):
+    self.packer = CANPacker(dbc_name)
+    self.enable_camera = enable_camera
+    self.enabled_last = False
+    self.main_on_last = False
+    self.vehicle_model = vehicle_model
+    self.generic_toggle_last = 0
+    self.steer_alert_last = False
+    self.lkas_action = 0
+
+  def update(self, enabled, CS, frame, actuators, visual_alert, pcm_cancel):
+
+    can_sends = []
+    steer_alert = visual_alert == car.CarControl.HUDControl.VisualAlert.steerRequired
+
+    apply_steer = actuators.steer
+
+    if self.enable_camera:
+
+      if pcm_cancel:
+        #print "CANCELING!!!!"
+        can_sends.append(spam_cancel_button(self.packer))
+
+      if (frame % 3) == 0:
+
+        curvature = self.vehicle_model.calc_curvature(actuators.steerAngle*3.1415/180., CS.v_ego)
+
+        # The use of the toggle below is handy for trying out the various LKAS modes
+        if TOGGLE_DEBUG:
+          self.lkas_action += int(CS.generic_toggle and not self.generic_toggle_last)
+          self.lkas_action &= 0xf
+        else:
+          self.lkas_action = 5   # 4 and 5 seem the best. 8 and 9 seem to aggressive and laggy
+
+        can_sends.append(create_steer_command(self.packer, apply_steer, enabled,
+                                              CS.lkas_state, CS.angle_steers, curvature, self.lkas_action))
+        self.generic_toggle_last = CS.generic_toggle
+
+      if (frame % 100) == 0:
+
+        can_sends.append(make_can_msg(973, '\x00\x00\x00\x00\x00\x00\x00\x00', 0, False))
+        #can_sends.append(make_can_msg(984, '\x00\x00\x00\x00\x80\x45\x60\x30', 0, False))
+
+      if (frame % 100) == 0 or (self.enabled_last != enabled) or (self.main_on_last != CS.main_on) or \
+         (self.steer_alert_last != steer_alert):
+        can_sends.append(create_lkas_ui(self.packer, CS.main_on, enabled, steer_alert))
+
+      if (frame % 200) == 0:
+        can_sends.append(make_can_msg(1875, '\x80\xb0\x55\x55\x78\x90\x00\x00', 1, False))
+
+      if (frame % 10) == 0:
+
+        can_sends.append(make_can_msg(1648, '\x00\x00\x00\x40\x00\x00\x50\x00', 1, False))
+        can_sends.append(make_can_msg(1649, '\x10\x10\xf1\x70\x04\x00\x00\x00', 1, False))
+
+        can_sends.append(make_can_msg(1664, '\x00\x00\x03\xe8\x00\x01\xa9\xb2', 1, False))
+        can_sends.append(make_can_msg(1674, '\x08\x00\x00\xff\x0c\xfb\x6a\x08', 1, False))
+        can_sends.append(make_can_msg(1675, '\x00\x00\x3b\x60\x37\x00\x00\x00', 1, False))
+        can_sends.append(make_can_msg(1690, '\x70\x00\x00\x55\x86\x1c\xe0\x00', 1, False))
+
+        can_sends.append(make_can_msg(1910, '\x06\x4b\x06\x4b\x42\xd3\x11\x30', 1, False))
+        can_sends.append(make_can_msg(1911, '\x48\x53\x37\x54\x48\x53\x37\x54', 1, False))
+        can_sends.append(make_can_msg(1912, '\x31\x34\x47\x30\x38\x31\x43\x42', 1, False))
+        can_sends.append(make_can_msg(1913, '\x31\x34\x47\x30\x38\x32\x43\x42', 1, False))
+        can_sends.append(make_can_msg(1969, '\xf4\x40\x00\x00\x00\x00\x00\x00', 1, False))
+        can_sends.append(make_can_msg(1971, '\x0b\xc0\x00\x00\x00\x00\x00\x00', 1, False))
+
+      static_msgs = range(1653, 1658)
+      for addr in static_msgs:
+        cnt = (frame % 10) + 1
+        can_sends.append(make_can_msg(addr, chr(cnt<<4) + '\x00\x00\x00\x00\x00\x00\x00', 1, False))
+
+      self.enabled_last = enabled
+      self.main_on_last = CS.main_on
+      self.steer_alert_last = steer_alert
+
+    return can_sends

selfdrive/car/ford/fordcan.py

@@ -0,0 +1,54 @@
+from common.numpy_fast import clip
+from selfdrive.car.ford.values import MAX_ANGLE
+
+
+def make_can_msg(addr, dat, alt, cks=False):
+  return [addr, 0, dat, alt]
+
+
+def create_steer_command(packer, angle_cmd, enabled, lkas_state, angle_steers, curvature, lkas_action):
+  """Creates a CAN message for the Ford Steer Command."""
+
+  #if enabled and lkas available:
+  if enabled and lkas_state in [2,3]: #and (frame % 500) >= 3:
+    action = lkas_action
+  else:
+    action = 0xf
+    angle_cmd = angle_steers/MAX_ANGLE
+
+  angle_cmd = clip(angle_cmd * MAX_ANGLE, - MAX_ANGLE, MAX_ANGLE)
+
+  values = {
+    "Lkas_Action": action,
+    "Lkas_Alert": 0xf,             # no alerts
+    "Lane_Curvature": clip(curvature, -0.01, 0.01),   # is it just for debug?
+    #"Lane_Curvature": 0,   # is it just for debug?
+    "Steer_Angle_Req": angle_cmd
+  }
+  return packer.make_can_msg("Lane_Keep_Assist_Control", 0, values)
+
+
+def create_lkas_ui(packer, main_on, enabled, steer_alert):
+  """Creates a CAN message for the Ford Steer Ui."""
+
+  if not main_on:
+    lines = 0xf
+  elif enabled:
+    lines = 0x3
+  else:
+    lines = 0x6
+
+  values = {
+    "Set_Me_X80": 0x80,
+    "Set_Me_X45": 0x45,
+    "Set_Me_X30": 0x30,
+    "Lines_Hud": lines,
+    "Hands_Warning_W_Chime": steer_alert,
+  }
+  return packer.make_can_msg("Lane_Keep_Assist_Ui", 0, values)
+
+def spam_cancel_button(packer):
+  values = {
+    "Cancel": 1
+  }
+  return packer.make_can_msg("Steering_Buttons", 0, values)

selfdrive/car/ford/interface.py

@@ -1,5 +1,4 @@
 #!/usr/bin/env python
-from common.realtime import sec_since_boot
 from cereal import car
 from selfdrive.swaglog import cloudlog
 from selfdrive.config import Conversions as CV
@@ -48,6 +47,7 @@ class CarInterface(object):
     ret.isPandaBlack = is_panda_black
 
     ret.safetyModel = car.CarParams.SafetyModel.ford
+    ret.dashcamOnly = True
 
     # pedal
     ret.enableCruise = True
@@ -108,7 +108,7 @@ class CarInterface(object):
   # returns a car.CarState
   def update(self, c, can_strings):
     # ******************* do can recv *******************
-    self.cp.update_strings(int(sec_since_boot() * 1e9), can_strings)
+    self.cp.update_strings(can_strings)
 
     self.CS.update(self.cp)
 

selfdrive/car/ford/radar_interface.py

@@ -3,7 +3,6 @@ import os
 import numpy as np
 from selfdrive.can.parser import CANParser
 from cereal import car
-from common.realtime import sec_since_boot
 
 RADAR_MSGS = range(0x500, 0x540)
 
@@ -32,8 +31,7 @@ class RadarInterface(object):
     self.updated_messages = set()
 
   def update(self, can_strings):
-    tm = int(sec_since_boot() * 1e9)
-    vls = self.rcp.update_strings(tm, can_strings)
+    vls = self.rcp.update_strings(can_strings)
     self.updated_messages.update(vls)
 
     if self.trigger_msg not in self.updated_messages:

selfdrive/car/gm/interface.py

@@ -1,6 +1,5 @@
 #!/usr/bin/env python
 from cereal import car
-from common.realtime import sec_since_boot
 from selfdrive.config import Conversions as CV
 from selfdrive.controls.lib.drive_helpers import create_event, EventTypes as ET
 from selfdrive.controls.lib.vehicle_model import VehicleModel
@@ -9,6 +8,7 @@ from selfdrive.car.gm.values import DBC, CAR, STOCK_CONTROL_MSGS, \
 from selfdrive.car.gm.carstate import CarState, CruiseButtons, get_powertrain_can_parser
 from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness
 
+ButtonType = car.CarState.ButtonEvent.Type
 
 class CanBus(object):
   def __init__(self):
@@ -62,6 +62,7 @@ class CarInterface(object):
     ret.enableCamera = not any(x for x in STOCK_CONTROL_MSGS[candidate] if x in fingerprint) or is_panda_black
     ret.openpilotLongitudinalControl = ret.enableCamera
     tire_stiffness_factor = 0.444  # not optimized yet
+    ret.safetyModelPassive = car.CarParams.SafetyModel.gmPassive
 
     if candidate == CAR.VOLT:
       # supports stop and go, but initial engage must be above 18mph (which include conservatism)
@@ -172,7 +173,7 @@ class CarInterface(object):
 
   # returns a car.CarState
   def update(self, c, can_strings):
-    self.pt_cp.update_strings(int(sec_since_boot() * 1e9), can_strings)
+    self.pt_cp.update_strings(can_strings)
 
     self.CS.update(self.pt_cp)
 
@@ -225,19 +226,19 @@ class CarInterface(object):
     # blinkers
     if self.CS.left_blinker_on != self.CS.prev_left_blinker_on:
       be = car.CarState.ButtonEvent.new_message()
-      be.type = 'leftBlinker'
+      be.type = ButtonType.leftBlinker
       be.pressed = self.CS.left_blinker_on
       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.type = ButtonType.rightBlinker
       be.pressed = self.CS.right_blinker_on
       buttonEvents.append(be)
 
     if self.CS.cruise_buttons != self.CS.prev_cruise_buttons:
       be = car.CarState.ButtonEvent.new_message()
-      be.type = 'unknown'
+      be.type = ButtonType.unknown
       if self.CS.cruise_buttons != CruiseButtons.UNPRESS:
         be.pressed = True
         but = self.CS.cruise_buttons
@@ -246,13 +247,13 @@ class CarInterface(object):
         but = self.CS.prev_cruise_buttons
       if but == CruiseButtons.RES_ACCEL:
         if not (cruiseEnabled and self.CS.standstill):
-          be.type = 'accelCruise' # Suppress resume button if we're resuming from stop so we don't adjust speed.
+          be.type = ButtonType.accelCruise # Suppress resume button if we're resuming from stop so we don't adjust speed.
       elif but == CruiseButtons.DECEL_SET:
-        be.type = 'decelCruise'
+        be.type = ButtonType.decelCruise
       elif but == CruiseButtons.CANCEL:
-        be.type = 'cancel'
+        be.type = ButtonType.cancel
       elif but == CruiseButtons.MAIN:
-        be.type = 'altButton3'
+        be.type = ButtonType.altButton3
       buttonEvents.append(be)
 
     ret.buttonEvents = buttonEvents
@@ -302,10 +303,10 @@ class CarInterface(object):
       # handle button presses
       for b in ret.buttonEvents:
         # do enable on both accel and decel buttons
-        if b.type in ["accelCruise", "decelCruise"] and not b.pressed:
+        if b.type in [ButtonType.accelCruise, ButtonType.decelCruise] and not b.pressed:
           events.append(create_event('buttonEnable', [ET.ENABLE]))
         # do disable on button down
-        if b.type == "cancel" and b.pressed:
+        if b.type == ButtonType.cancel and b.pressed:
           events.append(create_event('buttonCancel', [ET.USER_DISABLE]))
 
     ret.events = events

selfdrive/car/gm/radar_interface.py

@@ -6,7 +6,6 @@ from cereal import car
 from selfdrive.can.parser import CANParser
 from selfdrive.car.gm.interface import CanBus
 from selfdrive.car.gm.values import DBC, CAR
-from common.realtime import sec_since_boot
 
 RADAR_HEADER_MSG = 1120
 SLOT_1_MSG = RADAR_HEADER_MSG + 1
@@ -60,8 +59,7 @@ class RadarInterface(object):
       time.sleep(0.05)   # nothing to do
       return car.RadarData.new_message()
 
-    tm = int(sec_since_boot() * 1e9)
-    vls = self.rcp.update_strings(tm, can_strings)
+    vls = self.rcp.update_strings(can_strings)
     self.updated_messages.update(vls)
 
     if self.trigger_msg not in self.updated_messages:

selfdrive/car/honda/carstate.py

@@ -1,3 +1,4 @@
+from cereal import car
 from common.numpy_fast import interp
 from common.kalman.simple_kalman import KF1D
 from selfdrive.can.can_define import CANDefine
@@ -5,10 +6,12 @@ from selfdrive.can.parser import CANParser
 from selfdrive.config import Conversions as CV
 from selfdrive.car.honda.values import CAR, DBC, STEER_THRESHOLD, SPEED_FACTOR, HONDA_BOSCH
 
+GearShifter = car.CarState.GearShifter
+
 def parse_gear_shifter(gear, vals):
 
-  val_to_capnp = {'P': 'park', 'R': 'reverse', 'N': 'neutral',
-                  'D': 'drive', 'S': 'sport', 'L': 'low'}
+  val_to_capnp = {'P': GearShifter.park, 'R': GearShifter.reverse, 'N': GearShifter.neutral,
+                  'D': GearShifter.drive, 'S': GearShifter.sport, 'L': GearShifter.low}
   try:
     return val_to_capnp[vals[gear]]
   except KeyError:

selfdrive/car/honda/hondacan.py

@@ -47,7 +47,7 @@ def create_brake_command(packer, apply_brake, pump_on, pcm_override, pcm_cancel_
     "FCW": fcw << 1,
     "AEB_REQ_1": 0,
     "AEB_REQ_2": 0,
-    "AEB": 0,
+    "AEB_STATUS": 0,
   }
   bus = get_pt_bus(car_fingerprint, is_panda_black)
   return packer.make_can_msg("BRAKE_COMMAND", bus, values, idx)

selfdrive/car/honda/interface.py

@@ -3,7 +3,7 @@ import os
 import numpy as np
 from cereal import car
 from common.numpy_fast import clip, interp
-from common.realtime import sec_since_boot, DT_CTRL
+from common.realtime import DT_CTRL
 from selfdrive.swaglog import cloudlog
 from selfdrive.config import Conversions as CV
 from selfdrive.controls.lib.drive_helpers import create_event, EventTypes as ET, get_events
@@ -15,6 +15,8 @@ from selfdrive.controls.lib.planner import _A_CRUISE_MAX_V_FOLLOWING
 
 A_ACC_MAX = max(_A_CRUISE_MAX_V_FOLLOWING)
 
+ButtonType = car.CarState.ButtonEvent.Type
+GearShifter = car.CarState.GearShifter
 
 def compute_gb_honda(accel, speed):
   creep_brake = 0.0
@@ -374,8 +376,8 @@ class CarInterface(object):
   # returns a car.CarState
   def update(self, c, can_strings):
     # ******************* do can recv *******************
-    self.cp.update_strings(int(sec_since_boot() * 1e9), can_strings)
-    self.cp_cam.update_strings(int(sec_since_boot() * 1e9), can_strings)
+    self.cp.update_strings(can_strings)
+    self.cp_cam.update_strings(can_strings)
 
     self.CS.update(self.cp, self.cp_cam)
 
@@ -438,19 +440,19 @@ class CarInterface(object):
 
     if self.CS.left_blinker_on != self.CS.prev_left_blinker_on:
       be = car.CarState.ButtonEvent.new_message()
-      be.type = 'leftBlinker'
+      be.type = ButtonType.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.type = ButtonType.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'
+      be.type = ButtonType.unknown
       if self.CS.cruise_buttons != 0:
         be.pressed = True
         but = self.CS.cruise_buttons
@@ -458,18 +460,18 @@ class CarInterface(object):
         be.pressed = False
         but = self.CS.prev_cruise_buttons
       if but == CruiseButtons.RES_ACCEL:
-        be.type = 'accelCruise'
+        be.type = ButtonType.accelCruise
       elif but == CruiseButtons.DECEL_SET:
-        be.type = 'decelCruise'
+        be.type = ButtonType.decelCruise
       elif but == CruiseButtons.CANCEL:
-        be.type = 'cancel'
+        be.type = ButtonType.cancel
       elif but == CruiseButtons.MAIN:
-        be.type = 'altButton3'
+        be.type = ButtonType.altButton3
       buttonEvents.append(be)
 
     if self.CS.cruise_setting != self.CS.prev_cruise_setting:
       be = car.CarState.ButtonEvent.new_message()
-      be.type = 'unknown'
+      be.type = ButtonType.unknown
       if self.CS.cruise_setting != 0:
         be.pressed = True
         but = self.CS.cruise_setting
@@ -477,7 +479,7 @@ class CarInterface(object):
         be.pressed = False
         but = self.CS.prev_cruise_setting
       if but == 1:
-        be.type = 'altButton1'
+        be.type = ButtonType.altButton1
       # TODO: more buttons?
       buttonEvents.append(be)
     ret.buttonEvents = buttonEvents
@@ -493,7 +495,7 @@ class CarInterface(object):
       events.append(create_event('steerTempUnavailable', [ET.WARNING]))
     if self.CS.brake_error:
       events.append(create_event('brakeUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT]))
-    if not ret.gearShifter == 'drive':
+    if not ret.gearShifter == GearShifter.drive:
       events.append(create_event('wrongGear', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
     if ret.doorOpen:
       events.append(create_event('doorOpen', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
@@ -503,7 +505,7 @@ class CarInterface(object):
       events.append(create_event('espDisabled', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
     if not self.CS.main_on or self.CS.cruise_mode:
       events.append(create_event('wrongCarMode', [ET.NO_ENTRY, ET.USER_DISABLE]))
-    if ret.gearShifter == 'reverse':
+    if ret.gearShifter == GearShifter.reverse:
       events.append(create_event('reverseGear', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
     if self.CS.brake_hold and self.CS.CP.carFingerprint not in HONDA_BOSCH:
       events.append(create_event('brakeHold', [ET.NO_ENTRY, ET.USER_DISABLE]))
@@ -538,7 +540,7 @@ class CarInterface(object):
     for b in ret.buttonEvents:
 
       # do enable on both accel and decel buttons
-      if b.type in ["accelCruise", "decelCruise"] and not b.pressed:
+      if b.type in [ButtonType.accelCruise, ButtonType.decelCruise] and not b.pressed:
         self.last_enable_pressed = cur_time
         enable_pressed = True
 

selfdrive/car/honda/radar_interface.py

@@ -3,7 +3,6 @@ import os
 import time
 from cereal import car
 from selfdrive.can.parser import CANParser
-from common.realtime import sec_since_boot
 
 def _create_nidec_can_parser():
   dbc_f = 'acura_ilx_2016_nidec.dbc'
@@ -38,11 +37,11 @@ class RadarInterface(object):
     # in Bosch radar and we are only steering for now, so sleep 0.05s to keep
     # radard at 20Hz and return no points
     if self.radar_off_can:
+      if 'NO_RADAR_SLEEP' not in os.environ:
         time.sleep(0.05)
       return car.RadarData.new_message()
 
-    tm = int(sec_since_boot() * 1e9)
-    vls = self.rcp.update_strings(tm, can_strings)
+    vls = self.rcp.update_strings(can_strings)
     self.updated_messages.update(vls)
 
     if self.trigger_msg not in self.updated_messages:

selfdrive/car/honda/values.py

@@ -96,7 +96,7 @@ FINGERPRINTS = {
     57: 3, 148: 8, 228: 5, 229: 4, 304: 8, 342: 6, 344: 8, 380: 8, 399: 7, 411: 5, 419: 8, 420: 8, 427: 3, 432: 7, 440: 8, 450: 8, 463: 8, 464: 8, 476: 4, 490: 8, 506: 8, 507: 1, 542: 7, 545: 6, 597: 8, 662: 4, 773: 7, 777: 8, 780: 8, 795: 8, 800: 8, 804: 8, 806: 8, 808: 8, 817: 4, 819: 7, 821: 5, 825: 4, 829: 5, 837: 5, 856: 7, 862: 8, 871: 8, 881: 8, 882: 4, 884: 8, 891: 8, 892: 8, 905: 8, 923: 2, 927: 8, 929: 8, 963: 8, 965: 8, 966: 8, 967: 8, 983: 8, 985: 3, 1029: 8, 1036: 8, 1052: 8, 1064: 7, 1088: 8, 1089: 8, 1092: 1, 1108: 8, 1110: 8, 1125: 8, 1296: 8, 1302: 8, 1600: 5, 1601: 8, 1612: 5, 1613: 5, 1614: 5, 1616: 5, 1619: 5, 1623: 5, 1668: 5
   }],
   CAR.ODYSSEY_CHN: [{
-    57: 3, 145: 8, 316: 8, 342: 6, 344: 8, 380: 8, 398: 3, 399: 7, 401: 8, 404: 4, 411: 5, 420: 8, 422: 8, 423: 2, 426: 8, 432: 7, 450: 8, 464: 8, 490: 8, 506: 8, 507: 1, 597: 8, 610: 8, 611: 8, 612: 8, 617: 8, 660: 8, 661: 4, 773: 7, 780: 8, 804: 8, 808: 8, 829: 5, 862: 8, 884: 7, 892: 8, 923: 2, 929: 8, 1030: 5, 1137: 8, 1302: 8, 1348: 5, 1361: 5, 1365: 5, 1600: 5, 1601: 8, 1639: 8
+    57: 3, 145: 8, 316: 8, 342: 6, 344: 8, 380: 8, 398: 3, 399: 7, 401: 8, 404: 4, 411: 5, 420: 8, 422: 8, 423: 2, 426: 8, 432: 7, 450: 8, 464: 8, 490: 8, 506: 8, 507: 1, 512: 6, 513: 6, 597: 8, 610: 8, 611: 8, 612: 8, 617: 8, 660: 8, 661: 4, 773: 7, 780: 8, 804: 8, 808: 8, 829: 5, 862: 8, 884: 7, 892: 8, 923: 2, 929: 8, 1030: 5, 1137: 8, 1302: 8, 1348: 5, 1361: 5, 1365: 5, 1600: 5, 1601: 8, 1639: 8
   }],
   # 2017 Pilot Touring AND 2016 Pilot EX-L w/ Added Comma Pedal Support (512L & 513L)
   CAR.PILOT: [{
@@ -139,7 +139,7 @@ DBC = {
   CAR.CRV_HYBRID: dbc_dict('honda_crv_hybrid_2019_can_generated', None),
   CAR.FIT: dbc_dict('honda_fit_ex_2018_can_generated', 'acura_ilx_2016_nidec'),
   CAR.ODYSSEY: dbc_dict('honda_odyssey_exl_2018_generated', 'acura_ilx_2016_nidec'),
-  CAR.ODYSSEY_CHN: dbc_dict('honda_odyssey_extreme_edition_2018_china_can', 'acura_ilx_2016_nidec'),
+  CAR.ODYSSEY_CHN: dbc_dict('honda_odyssey_extreme_edition_2018_china_can_generated', 'acura_ilx_2016_nidec'),
   CAR.PILOT: dbc_dict('honda_pilot_touring_2017_can_generated', 'acura_ilx_2016_nidec'),
   CAR.PILOT_2019: dbc_dict('honda_pilot_touring_2017_can_generated', 'acura_ilx_2016_nidec'),
   CAR.RIDGELINE: dbc_dict('honda_ridgeline_black_edition_2017_can_generated', 'acura_ilx_2016_nidec'),

selfdrive/car/hyundai/interface.py

@@ -1,6 +1,5 @@
 #!/usr/bin/env python
 from cereal import car
-from common.realtime import sec_since_boot
 from selfdrive.config import Conversions as CV
 from selfdrive.controls.lib.drive_helpers import EventTypes as ET, create_event
 from selfdrive.controls.lib.vehicle_model import VehicleModel
@@ -8,6 +7,8 @@ from selfdrive.car.hyundai.carstate import CarState, get_can_parser, get_camera_
 from selfdrive.car.hyundai.values import CAMERA_MSGS, CAR, get_hud_alerts, FEATURES
 from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness
 
+GearShifter = car.CarState.GearShifter
+ButtonType = car.CarState.ButtonEvent.Type
 
 class CarInterface(object):
   def __init__(self, CP, CarController):
@@ -154,8 +155,8 @@ class CarInterface(object):
   # returns a car.CarState
   def update(self, c, can_strings):
     # ******************* do can recv *******************
-    self.cp.update_strings(int(sec_since_boot() * 1e9), can_strings)
-    self.cp_cam.update_strings(int(sec_since_boot() * 1e9), can_strings)
+    self.cp.update_strings(can_strings)
+    self.cp_cam.update_strings(can_strings)
 
     self.CS.update(self.cp, self.cp_cam)
     # create message
@@ -212,13 +213,13 @@ class CarInterface(object):
 
     if self.CS.left_blinker_on != self.CS.prev_left_blinker_on:
       be = car.CarState.ButtonEvent.new_message()
-      be.type = 'leftBlinker'
+      be.type = ButtonType.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.type = ButtonType.rightBlinker
       be.pressed = self.CS.right_blinker_on != 0
       buttonEvents.append(be)
 
@@ -236,7 +237,7 @@ class CarInterface(object):
       self.low_speed_alert = False
 
     events = []
-    if not ret.gearShifter == 'drive':
+    if not ret.gearShifter == GearShifter.drive:
       events.append(create_event('wrongGear', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
     if ret.doorOpen:
       events.append(create_event('doorOpen', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
@@ -246,12 +247,11 @@ class CarInterface(object):
       events.append(create_event('espDisabled', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
     if not self.CS.main_on:
       events.append(create_event('wrongCarMode', [ET.NO_ENTRY, ET.USER_DISABLE]))
-    if ret.gearShifter == 'reverse':
+    if ret.gearShifter == GearShifter.reverse:
       events.append(create_event('reverseGear', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
     if self.CS.steer_error:
       events.append(create_event('steerTempUnavailable', [ET.NO_ENTRY, ET.WARNING]))
 
-    # enable request in prius is simple, as we activate when Toyota is active (rising edge)
     if ret.cruiseState.enabled and not self.cruise_enabled_prev:
       events.append(create_event('pcmEnable', [ET.ENABLE]))
     elif not ret.cruiseState.enabled:

selfdrive/car/hyundai/radar_interface.py

@@ -1,7 +1,7 @@
 #!/usr/bin/env python
-from cereal import car
+import os
 import time
-
+from cereal import car
 
 class RadarInterface(object):
   def __init__(self, CP):
@@ -11,6 +11,8 @@ class RadarInterface(object):
 
   def update(self, can_strings):
     ret = car.RadarData.new_message()
+
+    if 'NO_RADAR_SLEEP' not in os.environ:
       time.sleep(0.05)  # radard runs on RI updates
 
     return ret

selfdrive/car/subaru/interface.py

@@ -1,6 +1,5 @@
 #!/usr/bin/env python
 from cereal import car
-from common.realtime import sec_since_boot
 from selfdrive.config import Conversions as CV
 from selfdrive.controls.lib.drive_helpers import create_event, EventTypes as ET
 from selfdrive.controls.lib.vehicle_model import VehicleModel
@@ -8,6 +7,7 @@ from selfdrive.car.subaru.values import CAR
 from selfdrive.car.subaru.carstate import CarState, get_powertrain_can_parser, get_camera_can_parser
 from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness
 
+ButtonType = car.CarState.ButtonEvent.Type
 
 class CarInterface(object):
   def __init__(self, CP, CarController):
@@ -42,6 +42,7 @@ class CarInterface(object):
     ret = car.CarParams.new_message()
 
     ret.carName = "subaru"
+    ret.radarOffCan = True
     ret.carFingerprint = candidate
     ret.carVin = vin
     ret.isPandaBlack = is_panda_black
@@ -96,8 +97,8 @@ class CarInterface(object):
 
   # returns a car.CarState
   def update(self, c, can_strings):
-    self.pt_cp.update_strings(int(sec_since_boot() * 1e9), can_strings)
-    self.cam_cp.update_strings(int(sec_since_boot() * 1e9), can_strings)
+    self.pt_cp.update_strings(can_strings)
+    self.cam_cp.update_strings(can_strings)
 
     self.CS.update(self.pt_cp, self.cam_cp)
 
@@ -144,18 +145,18 @@ class CarInterface(object):
     # blinkers
     if self.CS.left_blinker_on != self.CS.prev_left_blinker_on:
       be = car.CarState.ButtonEvent.new_message()
-      be.type = 'leftBlinker'
+      be.type = ButtonType.leftBlinker
       be.pressed = self.CS.left_blinker_on
       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.type = ButtonType.rightBlinker
       be.pressed = self.CS.right_blinker_on
       buttonEvents.append(be)
 
     be = car.CarState.ButtonEvent.new_message()
-    be.type = 'accelCruise'
+    be.type = ButtonType.accelCruise
     buttonEvents.append(be)
 
 

selfdrive/car/subaru/radar_interface.py

@@ -1,7 +1,7 @@
 #!/usr/bin/env python
-from cereal import car
+import os
 import time
-
+from cereal import car
 
 class RadarInterface(object):
   def __init__(self, CP):
@@ -10,8 +10,9 @@ class RadarInterface(object):
     self.delay = 0.1
 
   def update(self, can_strings):
-
     ret = car.RadarData.new_message()
+    
+    if 'NO_RADAR_SLEEP' not in os.environ:
       time.sleep(0.05)  # radard runs on RI updates
 
     return ret

selfdrive/car/toyota/carcontroller.py

@@ -5,8 +5,8 @@ from selfdrive.car import create_gas_command
 from selfdrive.car.toyota.toyotacan import make_can_msg, create_video_target,\
                                            create_steer_command, create_ui_command, \
                                            create_ipas_steer_command, create_accel_command, \
-                                           create_fcw_command
-from selfdrive.car.toyota.values import ECU, STATIC_MSGS, TSS2_CAR
+                                           create_acc_cancel_command, create_fcw_command
+from selfdrive.car.toyota.values import CAR, ECU, STATIC_MSGS, TSS2_CAR
 from selfdrive.can.packer import CANPacker
 
 VisualAlert = car.CarControl.HUDControl.VisualAlert
@@ -209,7 +209,11 @@ class CarController(object):
     # accel cmd comes from DSU, but we can spam can to cancel the system even if we are using lat only control
     if (frame % 3 == 0 and ECU.DSU in self.fake_ecus) or (pcm_cancel_cmd and ECU.CAM in self.fake_ecus):
       lead = lead or CS.v_ego < 12.    # at low speed we always assume the lead is present do ACC can be engaged
-      if ECU.DSU in self.fake_ecus:
+
+      # Lexus IS uses a different cancellation message
+      if pcm_cancel_cmd and CS.CP.carFingerprint == CAR.LEXUS_IS:
+        can_sends.append(create_acc_cancel_command(self.packer))
+      elif ECU.DSU in self.fake_ecus:
         can_sends.append(create_accel_command(self.packer, apply_accel, pcm_cancel_cmd, self.standstill_req, lead))
       else:
         can_sends.append(create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead))
@@ -236,8 +240,12 @@ class CarController(object):
     else:
       send_ui = False
 
+    # disengage msg causes a bad fault sound so play a good sound instead
+    if pcm_cancel_cmd:
+      send_ui = True
+
     if (frame % 100 == 0 or send_ui) and ECU.CAM in self.fake_ecus:
-      can_sends.append(create_ui_command(self.packer, steer, left_line, right_line, left_lane_depart, right_lane_depart))
+      can_sends.append(create_ui_command(self.packer, steer, pcm_cancel_cmd, left_line, right_line, left_lane_depart, right_lane_depart))
 
     if frame % 100 == 0 and ECU.DSU in self.fake_ecus and self.car_fingerprint not in TSS2_CAR:
       can_sends.append(create_fcw_command(self.packer, fcw))

selfdrive/car/toyota/carstate.py

@@ -1,14 +1,17 @@
 import numpy as np
+from cereal import car
 from common.kalman.simple_kalman import KF1D
 from selfdrive.can.can_define import CANDefine
 from selfdrive.can.parser import CANParser
 from selfdrive.config import Conversions as CV
 from selfdrive.car.toyota.values import CAR, DBC, STEER_THRESHOLD, TSS2_CAR, NO_DSU_CAR
 
+GearShifter = car.CarState.GearShifter
+
 def parse_gear_shifter(gear, vals):
 
-  val_to_capnp = {'P': 'park', 'R': 'reverse', 'N': 'neutral',
-                  'D': 'drive', 'B': 'brake'}
+  val_to_capnp = {'P': GearShifter.park, 'R': GearShifter.reverse, 'N': GearShifter.neutral,
+                  'D': GearShifter.drive, 'B': GearShifter.brake}
   try:
     return val_to_capnp[vals[gear]]
   except KeyError:
@@ -37,9 +40,6 @@ def get_can_parser(CP):
     ("STEER_RATE", "STEER_ANGLE_SENSOR", 0),
     ("CRUISE_ACTIVE", "PCM_CRUISE", 0),
     ("CRUISE_STATE", "PCM_CRUISE", 0),
-    ("MAIN_ON", "PCM_CRUISE_2", 0),
-    ("SET_SPEED", "PCM_CRUISE_2", 0),
-    ("LOW_SPEED_LOCKOUT", "PCM_CRUISE_2", 0),
     ("STEER_TORQUE_DRIVER", "STEER_TORQUE_SENSOR", 0),
     ("STEER_TORQUE_EPS", "STEER_TORQUE_SENSOR", 0),
     ("TURN_SIGNALS", "STEERING_LEVERS", 3),   # 3 is no blinkers
@@ -55,11 +55,20 @@ def get_can_parser(CP):
     ("WHEEL_SPEEDS", 80),
     ("STEER_ANGLE_SENSOR", 80),
     ("PCM_CRUISE", 33),
-    ("PCM_CRUISE_2", 33),
     ("STEER_TORQUE_SENSOR", 50),
     ("EPS_STATUS", 25),
   ]
 
+  if CP.carFingerprint == CAR.LEXUS_IS:
+    signals.append(("MAIN_ON", "DSU_CRUISE", 0))
+    signals.append(("SET_SPEED", "DSU_CRUISE", 0))
+    checks.append(("DSU_CRUISE", 5))
+  else:
+    signals.append(("MAIN_ON", "PCM_CRUISE_2", 0))
+    signals.append(("SET_SPEED", "PCM_CRUISE_2", 0))
+    signals.append(("LOW_SPEED_LOCKOUT", "PCM_CRUISE_2", 0))
+    checks.append(("PCM_CRUISE_2", 33))
+
   if CP.carFingerprint in NO_DSU_CAR:
     signals += [("STEER_ANGLE", "STEER_TORQUE_SENSOR", 0)]
 
@@ -158,6 +167,9 @@ class CarState(object):
     self.angle_steers_rate = cp.vl["STEER_ANGLE_SENSOR"]['STEER_RATE']
     can_gear = int(cp.vl["GEAR_PACKET"]['GEAR'])
     self.gear_shifter = parse_gear_shifter(can_gear, self.shifter_values)
+    if self.CP.carFingerprint == CAR.LEXUS_IS:
+      self.main_on = cp.vl["DSU_CRUISE"]['MAIN_ON']
+    else:
       self.main_on = cp.vl["PCM_CRUISE_2"]['MAIN_ON']
     self.left_blinker_on = cp.vl["STEERING_LEVERS"]['TURN_SIGNALS'] == 1
     self.right_blinker_on = cp.vl["STEERING_LEVERS"]['TURN_SIGNALS'] == 2
@@ -173,10 +185,14 @@ class CarState(object):
     self.steer_override = abs(self.steer_torque_driver) > STEER_THRESHOLD
 
     self.user_brake = 0
+    if self.CP.carFingerprint == CAR.LEXUS_IS:
+      self.v_cruise_pcm = cp.vl["DSU_CRUISE"]['SET_SPEED']
+      self.low_speed_lockout = False
+    else:
       self.v_cruise_pcm = cp.vl["PCM_CRUISE_2"]['SET_SPEED']
+      self.low_speed_lockout = cp.vl["PCM_CRUISE_2"]['LOW_SPEED_LOCKOUT'] == 2
     self.pcm_acc_status = cp.vl["PCM_CRUISE"]['CRUISE_STATE']
     self.pcm_acc_active = bool(cp.vl["PCM_CRUISE"]['CRUISE_ACTIVE'])
-    self.low_speed_lockout = cp.vl["PCM_CRUISE_2"]['LOW_SPEED_LOCKOUT'] == 2
     self.brake_lights = bool(cp.vl["ESP_CONTROL"]['BRAKE_LIGHTS_ACC'] or self.brake_pressed)
     if self.CP.carFingerprint == CAR.PRIUS:
       self.generic_toggle = cp.vl["AUTOPARK_STATUS"]['STATE'] != 0

selfdrive/car/toyota/interface.py

@@ -1,14 +1,16 @@
 #!/usr/bin/env python
-from common.realtime import sec_since_boot
 from cereal import car
 from selfdrive.config import Conversions as CV
 from selfdrive.controls.lib.drive_helpers import EventTypes as ET, create_event
 from selfdrive.controls.lib.vehicle_model import VehicleModel
 from selfdrive.car.toyota.carstate import CarState, get_can_parser, get_cam_can_parser
-from selfdrive.car.toyota.values import ECU, check_ecu_msgs, CAR, NO_STOP_TIMER_CAR
+from selfdrive.car.toyota.values import ECU, check_ecu_msgs, CAR, NO_STOP_TIMER_CAR, TSS2_CAR
 from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness
 from selfdrive.swaglog import cloudlog
 
+ButtonType = car.CarState.ButtonEvent.Type
+GearShifter = car.CarState.GearShifter
+
 class CarInterface(object):
   def __init__(self, CP, CarController):
     self.CP = CP
@@ -207,6 +209,16 @@ class CarInterface(object):
       ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.3], [0.05]]
       ret.lateralTuning.pid.kf = 0.00007818594
 
+    elif candidate == CAR.LEXUS_IS:
+      stop_and_go = False
+      ret.safetyParam = 66
+      ret.wheelbase = 2.79908
+      ret.steerRatio = 13.3
+      tire_stiffness_factor = 0.444
+      ret.mass = 3736.8 * CV.LB_TO_KG + STD_CARGO_KG
+      ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.3], [0.05]]
+      ret.lateralTuning.pid.kf = 0.00006
+
     ret.steerRateCost = 1.
     ret.centerToFront = ret.wheelbase * 0.44
 
@@ -236,8 +248,8 @@ class CarInterface(object):
     ret.brakeMaxBP = [0.]
     ret.brakeMaxV = [1.]
 
-    ret.enableCamera = not check_ecu_msgs(fingerprint, ECU.CAM) or is_panda_black
-    ret.enableDsu = not check_ecu_msgs(fingerprint, ECU.DSU)
+    ret.enableCamera = not check_ecu_msgs(fingerprint, ECU.CAM, candidate) or is_panda_black
+    ret.enableDsu = not check_ecu_msgs(fingerprint, ECU.DSU, candidate) or (is_panda_black and candidate in TSS2_CAR)
     ret.enableApgs = False #not check_ecu_msgs(fingerprint, ECU.APGS)
     ret.openpilotLongitudinalControl = ret.enableCamera and ret.enableDsu
     cloudlog.warn("ECU Camera Simulated: %r", ret.enableCamera)
@@ -270,11 +282,11 @@ class CarInterface(object):
   # returns a car.CarState
   def update(self, c, can_strings):
     # ******************* do can recv *******************
-    self.cp.update_strings(int(sec_since_boot() * 1e9), can_strings)
+    self.cp.update_strings(can_strings)
 
     # run the cam can update for 10s as we just need to know if the camera is alive
     if self.frame < 1000:
-      self.cp_cam.update_strings(int(sec_since_boot() * 1e9), can_strings)
+      self.cp_cam.update_strings(can_strings)
 
     self.CS.update(self.cp)
 
@@ -335,13 +347,13 @@ class CarInterface(object):
     buttonEvents = []
     if self.CS.left_blinker_on != self.CS.prev_left_blinker_on:
       be = car.CarState.ButtonEvent.new_message()
-      be.type = 'leftBlinker'
+      be.type = ButtonType.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.type = ButtonType.rightBlinker
       be.pressed = self.CS.right_blinker_on != 0
       buttonEvents.append(be)
 
@@ -359,7 +371,7 @@ class CarInterface(object):
     if self.cp_cam.can_valid:
       self.forwarding_camera = True
 
-    if not ret.gearShifter == 'drive' and self.CP.enableDsu:
+    if not ret.gearShifter == GearShifter.drive and self.CP.enableDsu:
       events.append(create_event('wrongGear', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
     if ret.doorOpen:
       events.append(create_event('doorOpen', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
@@ -369,7 +381,7 @@ class CarInterface(object):
       events.append(create_event('espDisabled', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
     if not self.CS.main_on and self.CP.enableDsu:
       events.append(create_event('wrongCarMode', [ET.NO_ENTRY, ET.USER_DISABLE]))
-    if ret.gearShifter == 'reverse' and self.CP.enableDsu:
+    if ret.gearShifter == GearShifter.reverse and self.CP.enableDsu:
       events.append(create_event('reverseGear', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
     if self.CS.steer_error:
       events.append(create_event('steerTempUnavailable', [ET.NO_ENTRY, ET.WARNING]))

selfdrive/car/toyota/radar_interface.py

@@ -3,7 +3,6 @@ import os
 import time
 from selfdrive.can.parser import CANParser
 from cereal import car
-from common.realtime import sec_since_boot
 from selfdrive.car.toyota.values import NO_DSU_CAR, DBC, TSS2_CAR
 
 def _create_radar_can_parser(car_fingerprint):
@@ -58,8 +57,7 @@ class RadarInterface(object):
       time.sleep(0.05)
       return car.RadarData.new_message()
 
-    tm = int(sec_since_boot() * 1e9)
-    vls = self.rcp.update_strings(tm, can_strings)
+    vls = self.rcp.update_strings(can_strings)
     self.updated_messages.update(vls)
 
     if self.trigger_msg not in self.updated_messages:

selfdrive/car/toyota/toyotacan.py

@@ -95,6 +95,18 @@ def create_accel_command(packer, accel, pcm_cancel, standstill_req, lead):
   return packer.make_can_msg("ACC_CONTROL", 0, values)
 
 
+def create_acc_cancel_command(packer):
+  values = {
+    "GAS_RELEASED": 0,
+    "CRUISE_ACTIVE": 0,
+    "STANDSTILL_ON": 0,
+    "ACCEL_NET": 0,
+    "CRUISE_STATE": 0,
+    "CANCEL_REQ": 1,
+  }
+  return packer.make_can_msg("PCM_CRUISE", 0, values)
+
+
 def create_fcw_command(packer, fcw):
   values = {
     "FCW": fcw,
@@ -105,7 +117,7 @@ def create_fcw_command(packer, fcw):
   return packer.make_can_msg("ACC_HUD", 0, values)
 
 
-def create_ui_command(packer, steer, left_line, right_line, left_lane_depart, right_lane_depart):
+def create_ui_command(packer, steer, chime, left_line, right_line, left_lane_depart, right_lane_depart):
   values = {
     "RIGHT_LINE": 3 if right_lane_depart else 1 if right_line else 2,
     "LEFT_LINE": 3 if left_lane_depart else 1 if left_line else 2,
@@ -117,7 +129,7 @@ def create_ui_command(packer, steer, left_line, right_line, left_lane_depart, ri
     "SET_ME_X01": 1,
     "SET_ME_X01_2": 1,
     "REPEATED_BEEPS": 0,
-    "TWO_BEEPS": 0,
+    "TWO_BEEPS": chime,
     "LDA_ALERT": steer,
   }
   return packer.make_can_msg("LKAS_HUD", 0, values)

selfdrive/car/toyota/values.py

@@ -17,6 +17,7 @@ class CAR:
   COROLLA_TSS2 = "TOYOTA COROLLA TSS2 2019"
   LEXUS_ESH_TSS2 = "LEXUS ES 300H 2019"
   SIENNA = "TOYOTA SIENNA XLE 2018"
+  LEXUS_IS = "LEXUS IS300 2018"
 
 
 class ECU:
@@ -77,9 +78,10 @@ ECU_FINGERPRINT = {
 }
 
 
-def check_ecu_msgs(fingerprint, ecu):
-  # return True if fingerprint contains messages normally sent by a given ecu
-  return ECU_FINGERPRINT[ecu] in fingerprint
+def check_ecu_msgs(fingerprint, ecu, car):
+  # return True if the reference car fingerprint doesn't contain the ecu fingerprint msg or
+  # fingerprint contains messages normally sent by a given ecu
+  return any(ECU_FINGERPRINT[ecu] not in finger for finger in FINGERPRINTS[car]) or ECU_FINGERPRINT[ecu] in fingerprint
 
 
 FINGERPRINTS = {
@@ -109,8 +111,9 @@ FINGERPRINTS = {
     36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 512: 6, 513:6, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 5, 643: 7, 658: 8, 713: 8, 740: 5, 742: 8, 743: 8, 800: 8, 810: 2, 812: 3, 814: 8, 830: 7, 835: 8, 836: 8, 845: 5, 863: 8, 869: 7, 870: 7, 871: 2, 898: 8, 900: 6, 902: 6, 905: 8, 913: 8, 918: 8, 921: 8, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 975: 6, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1005: 2, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1059: 1, 1063: 8, 1071: 8, 1077: 8, 1082: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1227: 8, 1228: 8, 1235: 8, 1237: 8, 1264: 8, 1279: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1575: 8, 1595: 8, 1777: 8, 1779: 8, 1808: 8, 1810: 8, 1816: 8, 1818: 8, 1840: 8, 1848: 8, 1904: 8, 1912: 8, 1940: 8, 1941: 8, 1948: 8, 1949: 8, 1952: 8, 1956: 8, 1960: 8, 1964: 8, 1986: 8, 1990: 8, 1994: 8, 1998: 8, 2004: 8, 2012: 8
   },
   # RX450HL
+  # TODO: get proper fingerprint in stock mode
   {
-    36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 512: 6, 513: 6, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 5, 643: 7, 658: 8, 713: 8, 742: 8, 743: 8, 800: 8, 810: 2, 812: 3, 814: 8, 830: 7, 835: 8, 836: 8, 863: 8, 865: 8, 869: 7, 870: 7, 898: 8, 900: 6, 902: 6, 905: 8, 918: 8, 921: 8, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 975: 6, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1005: 2, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1056: 8, 1057: 8, 1059: 1, 1063: 8, 1071: 8, 1076: 8, 1077: 8, 1082: 8, 1114: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1227: 8, 1228: 8, 1237: 8, 1264: 8, 1279: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1575: 8, 1592: 8, 1594: 8, 1595: 8, 1649: 8, 1777: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
+    36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 512: 6, 513: 6, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 5, 643: 7, 658: 8, 713: 8, 740: 5, 742: 8, 743: 8, 800: 8, 810: 2, 812: 3, 814: 8, 830: 7, 835: 8, 836: 8, 863: 8, 865: 8, 869: 7, 870: 7, 898: 8, 900: 6, 902: 6, 905: 8, 918: 8, 921: 8, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 975: 6, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1005: 2, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1056: 8, 1057: 8, 1059: 1, 1063: 8, 1071: 8, 1076: 8, 1077: 8, 1082: 8, 1114: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1227: 8, 1228: 8, 1237: 8, 1264: 8, 1279: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1575: 8, 1592: 8, 1594: 8, 1595: 8, 1649: 8, 1777: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
   },
   # RX540H 2019 with color hud
   {
@@ -128,8 +131,9 @@ FINGERPRINTS = {
     36: 8, 37: 8, 119: 6, 170: 8, 180: 8, 186: 4, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 550: 8, 552: 4, 562: 6, 608: 8, 610: 8, 643: 7, 658: 8, 705: 8, 728: 8, 740: 5, 761: 8, 764: 8, 800: 8, 810: 2, 812: 8, 814: 8, 818: 8, 822: 8, 824: 8, 830: 7, 835: 8, 836: 8, 869: 7, 870: 7, 871: 2, 888: 8, 889: 8, 891: 8, 898: 8, 900: 6, 902: 6, 905: 8, 918: 8, 921: 8, 933: 8, 934: 8, 935: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 976: 1, 983: 8, 984: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1011: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1059: 1, 1076: 8, 1077: 8, 1082: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1228: 8, 1235: 8, 1237: 8, 1263: 8, 1264: 8, 1279: 8, 1412: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1595: 8, 1745: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1808: 8, 1816: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
   },
   #XSE and SE
+  # TODO: get proper fingerprint in stock mode
   {
-    36: 8, 37: 8, 114: 5, 119: 6, 120: 4, 170: 8, 180: 8, 186: 4, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 550: 8, 552: 4, 562: 6, 608: 8, 610: 8, 643: 7, 658: 8, 705: 8, 728: 8, 761: 8, 764: 8, 800: 8, 810: 2, 812: 8, 814: 8, 818: 8, 822: 8, 824: 8, 830: 7, 835: 8, 836: 8, 869: 7, 870: 7, 888: 8, 889: 8, 891: 8, 898: 8, 900: 6, 902: 6, 905: 8, 918: 8, 921: 8, 933: 8, 934: 8, 935: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 976: 1, 983: 8, 984: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1011: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1056: 8, 1059: 1, 1076: 8, 1077: 8, 1082: 8, 1114: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1228: 8, 1237: 8, 1263: 8, 1264: 8, 1279: 8, 1412: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1595: 8, 1745: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1808: 8, 1816: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
+    36: 8, 37: 8, 114: 5, 119: 6, 120: 4, 170: 8, 180: 8, 186: 4, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 550: 8, 552: 4, 562: 6, 608: 8, 610: 8, 643: 7, 658: 8, 705: 8, 728: 8, 740: 5, 761: 8, 764: 8, 800: 8, 810: 2, 812: 8, 814: 8, 818: 8, 822: 8, 824: 8, 830: 7, 835: 8, 836: 8, 869: 7, 870: 7, 888: 8, 889: 8, 891: 8, 898: 8, 900: 6, 902: 6, 905: 8, 918: 8, 921: 8, 933: 8, 934: 8, 935: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 976: 1, 983: 8, 984: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1011: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1056: 8, 1059: 1, 1076: 8, 1077: 8, 1082: 8, 1114: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1228: 8, 1237: 8, 1263: 8, 1264: 8, 1279: 8, 1412: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1595: 8, 1745: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1808: 8, 1816: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
   }],
   CAR.CAMRYH: [
   #SE, LE and LE with Blindspot Monitor
@@ -182,6 +186,15 @@ FINGERPRINTS = {
   CAR.SIENNA: [{
     36: 8, 37: 8, 114: 5, 119: 6, 120: 4, 170: 8, 180: 8, 186: 4, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 545: 5, 548: 8, 550: 8, 552: 4, 562: 4, 608: 8, 610: 5, 643: 7, 705: 8, 725: 2, 740: 5, 764: 8, 800: 8, 824: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 888: 8, 896: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 1, 918: 7, 921: 8, 933: 8, 944: 6, 945: 8, 951: 8, 955: 8, 956: 8, 979: 2, 992: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1008: 2, 1014: 8, 1017: 8, 1041: 8, 1042: 8, 1043: 8, 1056: 8, 1059: 1, 1076: 8, 1077: 8, 1114: 8, 1160: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1182: 8, 1183: 8, 1191: 8, 1192: 8, 1196: 8, 1197: 8, 1198: 8, 1199: 8, 1200: 8, 1201: 8, 1202: 8, 1203: 8, 1212: 8, 1227: 8, 1228: 8, 1235: 8, 1237: 8, 1279: 8, 1552: 8, 1553: 8, 1555: 8, 1556: 8, 1557: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1584: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1656: 8, 1664: 8, 1666: 8, 1667: 8, 1728: 8, 1745: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
   }],
+  CAR.LEXUS_IS: [
+  # IS300 2018
+  {
+    36: 8, 37: 8, 114: 5, 119: 6, 120: 4, 170: 8, 180: 8, 186: 4, 238: 4, 400: 6, 426: 6, 452: 8, 464: 8, 466: 8, 467: 5, 544: 4, 550: 8, 552: 4, 608: 8, 610: 5, 643: 7, 705: 8, 740: 5, 800: 8, 836: 8, 845: 5, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 897: 8, 900: 6, 902: 6, 905: 8, 911: 8, 913: 8, 916: 3, 918: 7, 921: 8, 933: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 979: 2, 992: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1005: 2, 1008: 2, 1009: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1059: 1, 1112: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1168: 1, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1182: 8, 1183: 8, 1184: 8, 1185: 8, 1186: 8, 1187: 8, 1189: 8, 1190: 8, 1191: 8, 1192: 8, 1196: 8, 1197: 8, 1198: 8, 1199: 8, 1206: 8, 1208: 8, 1212: 8, 1227: 8, 1235: 8, 1237: 8, 1279: 8, 1408: 8, 1409: 8, 1410: 8, 1552: 8, 1553: 8, 1554: 8, 1555: 8, 1556: 8, 1557: 8, 1561: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1575: 8, 1584: 8, 1589: 8, 1590: 8, 1592: 8, 1593: 8, 1595: 8, 1599: 8, 1648: 8, 1666: 8, 1667: 8, 1728: 8, 1745: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
+  },
+  # IS300H 2017
+  {
+    36: 8, 37: 8, 170: 8, 180: 8, 295: 8, 296: 8, 400: 6, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 581: 5, 608: 8, 610: 5, 643: 7, 713: 8, 740: 5, 800: 8, 836: 8, 845: 5, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 897: 8, 900: 6, 902: 6, 905: 8, 911: 8, 913: 8, 916: 3, 918: 7, 921: 7, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 3, 955: 8, 956: 8, 979: 2, 992: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1009: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1112: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1168: 1, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1184: 8, 1185: 8, 1186: 8, 1187: 8, 1189: 8, 1190: 8, 1191: 8, 1192: 8, 1196: 8, 1197: 8, 1198: 8, 1199: 8, 1206: 8, 1208: 8, 1212: 8, 1227: 8, 1232: 8, 1235: 8, 1279: 8, 1408: 8, 1409: 8, 1410: 8, 1552: 8, 1553: 8, 1554: 8, 1555: 8, 1556: 8, 1557: 8, 1561: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1575: 8, 1584: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1599: 8, 1728: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
+  }],
 }
 
 STEER_THRESHOLD = 100
@@ -203,6 +216,7 @@ DBC = {
   CAR.COROLLA_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'),
   CAR.LEXUS_ESH_TSS2: dbc_dict('toyota_nodsu_hybrid_pt_generated', 'toyota_tss2_adas'),
   CAR.SIENNA: dbc_dict('toyota_sienna_xle_2018_pt_generated', 'toyota_adas'),
+  CAR.LEXUS_IS: dbc_dict('lexus_is_2018_pt_generated', 'toyota_adas'),
 }
 
 NO_DSU_CAR = [CAR.CHR, CAR.CHRH, CAR.CAMRY, CAR.CAMRYH, CAR.RAV4_TSS2, CAR.COROLLA_TSS2, CAR.LEXUS_ESH_TSS2]

selfdrive/common/touch.c

@@ -75,8 +75,9 @@ int touch_poll(TouchState *s, int* out_x, int* out_y, int timeout) {
         s->last_x = event.value;
       } else if (event.code == ABS_MT_POSITION_Y) {
         s->last_y = event.value;
-      }
+      } else if (event.code == ABS_MT_TRACKING_ID && event.value != -1) {
         up = true;
+      }
       break;
     default:
       break;
@@ -117,4 +118,3 @@ int touch_read(TouchState *s, int* out_x, int* out_y) {
   }
   return up;
 }
-

selfdrive/common/version.h

@@ -1 +1 @@
-#define COMMA_VERSION "0.6.3-release"
+#define COMMA_VERSION "0.6.4-release"

selfdrive/config.py

@@ -17,7 +17,8 @@ class Conversions:
   LB_TO_KG = 0.453592
 
 
-RADAR_TO_CENTER = 2.7   # RADAR is ~ 2.7m ahead from center of car
+RADAR_TO_CENTER = 2.7   # (deprecated) RADAR is ~ 2.7m ahead from center of car
+RADAR_TO_CAMERA = 1.52   # RADAR is ~ 1.5m ahead from center of mesh frame
 
 class UIParams:
   lidar_x, lidar_y, lidar_zoom = 384, 960, 6

selfdrive/controls/controlsd.py

@@ -6,12 +6,12 @@ from cereal import car, log
 from common.numpy_fast import clip
 from common.realtime import sec_since_boot, set_realtime_priority, Ratekeeper, DT_CTRL
 from common.profiler import Profiler
-from common.params import Params
+from common.params import Params, put_nonblocking
 import selfdrive.messaging as messaging
 from selfdrive.config import Conversions as CV
 from selfdrive.services import service_list
 from selfdrive.boardd.boardd import can_list_to_can_capnp
-from selfdrive.car.car_helpers import get_car, get_startup_alert
+from selfdrive.car.car_helpers import get_car, get_startup_alert, get_one_can
 from selfdrive.controls.lib.lane_planner import CAMERA_OFFSET
 from selfdrive.controls.lib.drive_helpers import get_events, \
                                                  create_event, \
@@ -26,6 +26,7 @@ from selfdrive.controls.lib.alertmanager import AlertManager
 from selfdrive.controls.lib.vehicle_model import VehicleModel
 from selfdrive.controls.lib.driver_monitor import DriverStatus, MAX_TERMINAL_ALERTS
 from selfdrive.controls.lib.planner import LON_MPC_STEP
+from selfdrive.controls.lib.gps_helpers import is_rhd_region
 from selfdrive.locationd.calibration_helpers import Calibration, Filter
 
 ThermalStatus = log.ThermalData.ThermalStatus
@@ -50,10 +51,6 @@ def events_to_bytes(events):
     ret.append(e.to_bytes())
   return ret
 
-def wait_for_can(logcan):
-  print("Waiting for CAN messages...")
-  while len(messaging.recv_one(logcan).can) == 0:
-    pass
 
 def data_sample(CI, CC, sm, can_sock, cal_status, cal_perc, overtemp, free_space, low_battery,
                 driver_status, state, mismatch_counter, params):
@@ -85,6 +82,12 @@ def data_sample(CI, CC, sm, can_sock, cal_status, cal_perc, overtemp, free_space
   if free_space:
     events.append(create_event('outOfSpace', [ET.NO_ENTRY]))
 
+  # GPS coords RHD parsing, once every restart
+  if sm.updated['gpsLocation'] and not driver_status.is_rhd_region_checked:
+    is_rhd = is_rhd_region(sm['gpsLocation'].latitude, sm['gpsLocation'].longitude)
+    driver_status.is_rhd_region = is_rhd
+    driver_status.is_rhd_region_checked = True
+    put_nonblocking("IsRHD", str(int(is_rhd)))
 
   # Handle calibration
   if sm.updated['liveCalibration']:
@@ -118,7 +121,7 @@ def data_sample(CI, CC, sm, can_sock, cal_status, cal_perc, overtemp, free_space
 
   # Driver monitoring
   if sm.updated['driverMonitoring']:
-    driver_status.get_pose(sm['driverMonitoring'], params, cal_rpy)
+    driver_status.get_pose(sm['driverMonitoring'], cal_rpy, CS.vEgo, enabled)
 
   if driver_status.terminal_alert_cnt >= MAX_TERMINAL_ALERTS:
     events.append(create_event("tooDistracted", [ET.NO_ENTRY]))
@@ -283,9 +286,8 @@ def state_control(frame, rcv_frame, plan, path_plan, CS, CP, state, events, v_cr
   return actuators, v_cruise_kph, driver_status, v_acc_sol, a_acc_sol, lac_log
 
 
-def data_send(sm, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, carstate,
-              carcontrol, carevents, carparams, controlsstate, sendcan, AM, driver_status,
-              LaC, LoC, read_only, start_time, v_acc, a_acc, lac_log, events_prev):
+def data_send(sm, pm, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, AM,
+              driver_status, LaC, LoC, read_only, start_time, v_acc, a_acc, lac_log, events_prev):
   """Send actuators and hud commands to the car, send controlsstate and MPC logging"""
 
   CC = car.CarControl.new_message()
@@ -324,7 +326,7 @@ def data_send(sm, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, ca
   if not read_only:
     # send car controls over can
     can_sends = CI.apply(CC)
-    sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan', valid=CS.canValid))
+    pm.send('sendcan', can_list_to_can_capnp(can_sends, msgtype='sendcan', valid=CS.canValid))
 
   force_decel = driver_status.awareness < 0.
 
@@ -341,7 +343,7 @@ def data_send(sm, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, ca
     "alertType": AM.alert_type,
     "alertSound": AM.audible_alert,
     "awarenessStatus": max(driver_status.awareness, 0.0) if isEnabled(state) else 0.0,
-    "driverMonitoringOn": bool(driver_status.monitor_on and driver_status.face_detected),
+    "driverMonitoringOn": bool(driver_status.face_detected),
     "canMonoTimes": list(CS.canMonoTimes),
     "planMonoTime": sm.logMonoTime['plan'],
     "pathPlanMonoTime": sm.logMonoTime['pathPlan'],
@@ -364,7 +366,6 @@ def data_send(sm, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, ca
     "vTargetLead": float(v_acc),
     "aTarget": float(a_acc),
     "jerkFactor": float(sm['plan'].jerkFactor),
-    "angleModelBias": 0.,
     "gpsPlannerActive": sm['plan'].gpsPlannerActive,
     "vCurvature": sm['plan'].vCurvature,
     "decelForModel": sm['plan'].longitudinalPlanSource == log.Plan.LongitudinalPlanSource.model,
@@ -380,7 +381,7 @@ def data_send(sm, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, ca
     dat.controlsState.lateralControlState.lqrState = lac_log
   elif CP.lateralTuning.which() == 'indi':
     dat.controlsState.lateralControlState.indiState = lac_log
-  controlsstate.send(dat.to_bytes())
+  pm.send('controlsState', dat)
 
   # carState
   cs_send = messaging.new_message()
@@ -388,7 +389,7 @@ def data_send(sm, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, ca
   cs_send.valid = CS.canValid
   cs_send.carState = CS
   cs_send.carState.events = events
-  carstate.send(cs_send.to_bytes())
+  pm.send('carState', cs_send)
 
   # carEvents - logged every second or on change
   events_bytes = events_to_bytes(events)
@@ -396,26 +397,26 @@ def data_send(sm, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, ca
     ce_send = messaging.new_message()
     ce_send.init('carEvents', len(events))
     ce_send.carEvents = events
-    carevents.send(ce_send.to_bytes())
+    pm.send('carEvents', ce_send)
 
   # carParams - logged every 50 seconds (> 1 per segment)
   if (sm.frame % int(50. / DT_CTRL) == 0):
     cp_send = messaging.new_message()
     cp_send.init('carParams')
     cp_send.carParams = CP
-    carparams.send(cp_send.to_bytes())
+    pm.send('carParams', cp_send)
 
   # carControl
   cc_send = messaging.new_message()
   cc_send.init('carControl')
   cc_send.valid = CS.canValid
   cc_send.carControl = CC
-  carcontrol.send(cc_send.to_bytes())
+  pm.send('carControl', cc_send)
 
   return CC, events_bytes
 
 
-def controlsd_thread(gctx=None):
+def controlsd_thread(sm=None, pm=None, can_sock=None):
   gc.disable()
 
   # start the loop
@@ -423,39 +424,35 @@ def controlsd_thread(gctx=None):
 
   params = Params()
 
-  # Pub Sockets
-  sendcan = messaging.pub_sock(service_list['sendcan'].port)
-  controlsstate = messaging.pub_sock(service_list['controlsState'].port)
-  carstate = messaging.pub_sock(service_list['carState'].port)
-  carcontrol = messaging.pub_sock(service_list['carControl'].port)
-  carevents = messaging.pub_sock(service_list['carEvents'].port)
-  carparams = messaging.pub_sock(service_list['carParams'].port)
-
   is_metric = params.get("IsMetric") == "1"
   passive = params.get("Passive") != "0"
 
-  sm = messaging.SubMaster(['thermal', 'health', 'liveCalibration', 'driverMonitoring', 'plan', 'pathPlan'])
+  # Pub/Sub Sockets
+  if pm is None:
+    pm = messaging.PubMaster(['sendcan', 'controlsState', 'carState', 'carControl', 'carEvents', 'carParams'])
+
+  if sm is None:
+    sm = messaging.SubMaster(['thermal', 'health', 'liveCalibration', 'driverMonitoring', 'plan', 'pathPlan', \
+                              'gpsLocation'], ignore_alive=['gpsLocation'])
 
-  logcan = messaging.sub_sock(service_list['can'].port)
+  if can_sock is None:
+    can_timeout = None if os.environ.get('NO_CAN_TIMEOUT', False) else 100
+    can_sock = messaging.sub_sock(service_list['can'].port, timeout=can_timeout)
 
   # wait for health and CAN packets
   hw_type = messaging.recv_one(sm.sock['health']).health.hwType
   is_panda_black = hw_type == log.HealthData.HwType.blackPanda
-  wait_for_can(logcan)
-
-  CI, CP = get_car(logcan, sendcan, is_panda_black)
-  logcan.close()
+  print("Waiting for CAN messages...")
+  get_one_can(can_sock)
 
-  # TODO: Use the logcan socket from above, but that will currenly break the tests
-  can_timeout = None if os.environ.get('NO_CAN_TIMEOUT', False) else 100
-  can_sock = messaging.sub_sock(service_list['can'].port, timeout=can_timeout)
+  CI, CP = get_car(can_sock, pm.sock['sendcan'], is_panda_black)
 
   car_recognized = CP.carName != 'mock'
   # If stock camera is disconnected, we loaded car controls and it's not chffrplus
   controller_available = CP.enableCamera and CI.CC is not None and not passive
-  read_only = not car_recognized or not controller_available
+  read_only = not car_recognized or not controller_available or CP.dashcamOnly
   if read_only:
-    CP.safetyModel = car.CarParams.SafetyModel.elm327   # diagnostic only
+    CP.safetyModel = CP.safetyModelPassive
 
   # Write CarParams for radard and boardd safety mode
   params.put("CarParams", CP.to_bytes())
@@ -478,6 +475,9 @@ def controlsd_thread(gctx=None):
     LaC = LatControlLQR(CP)
 
   driver_status = DriverStatus()
+  is_rhd = params.get("IsRHD")
+  if is_rhd is not None:
+    driver_status.is_rhd = bool(int(is_rhd))
 
   state = State.disabled
   soft_disable_timer = 0
@@ -550,16 +550,16 @@ def controlsd_thread(gctx=None):
     prof.checkpoint("State Control")
 
     # Publish data
-    CC, events_prev = data_send(sm, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, carstate, carcontrol, carevents, carparams,
-                   controlsstate, sendcan, AM, driver_status, LaC, LoC, read_only, start_time, v_acc, a_acc, lac_log, events_prev)
+    CC, events_prev = data_send(sm, pm, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, AM, driver_status, LaC,
+                                LoC, read_only, start_time, v_acc, a_acc, lac_log, events_prev)
     prof.checkpoint("Sent")
 
     rk.monitor_time()
     prof.display()
 
 
-def main(gctx=None):
-  controlsd_thread(gctx)
+def main(sm=None, pm=None, logcan=None):
+  controlsd_thread(sm, pm, logcan)
 
 
 if __name__ == "__main__":

selfdrive/controls/lib/drive_helpers.py

@@ -59,26 +59,6 @@ def get_steer_max(CP, v_ego):
   return interp(v_ego, CP.steerMaxBP, CP.steerMaxV)
 
 
-def learn_angle_model_bias(lateral_control, v_ego, angle_model_bias, c_poly, c_prob, angle_steers, steer_override):
-  # simple integral controller that learns how much steering offset to put to have the car going straight
-  # while being in the middle of the lane
-  min_offset = -5.  # deg
-  max_offset = 5.  # deg
-  alpha = 1. / 36000.  # correct by 1 deg in 2 mins, at 30m/s, with 50cm of error, at 20Hz
-  min_learn_speed = 1.
-
-  # learn less at low speed or when turning
-  slow_factor = 1. / (1. + 0.02 * abs(angle_steers) * v_ego)
-  alpha_v = alpha * c_prob * (max(v_ego - min_learn_speed, 0.)) * slow_factor
-
-  # only learn if lateral control is active and if driver is not overriding:
-  if lateral_control and not steer_override:
-    angle_model_bias += c_poly[3] * alpha_v
-    angle_model_bias = clip(angle_model_bias, min_offset, max_offset)
-
-  return angle_model_bias
-
-
 def update_v_cruise(v_cruise_kph, buttonEvents, enabled):
   # handle button presses. TODO: this should be in state_control, but a decelCruise press
   # would have the effect of both enabling and changing speed is checked after the state transition

selfdrive/controls/lib/driver_monitor.py

@@ -1,14 +1,15 @@
 import numpy as np
-from common.realtime import sec_since_boot, DT_CTRL, DT_DMON
+from common.realtime import DT_CTRL, DT_DMON
 from selfdrive.controls.lib.drive_helpers import create_event, EventTypes as ET
 from common.filter_simple import FirstOrderFilter
+from common.stat_live import RunningStatFilter
 
-_AWARENESS_TIME = 90.        # 1.5 minutes limit without user touching steering wheels make the car enter a terminal status
-_AWARENESS_PRE_TIME_TILL_TERMINAL = 20.    # a first alert is issued 20s before expiration
-_AWARENESS_PROMPT_TIME_TILL_TERMINAL = 5.  # a second alert is issued 5s before start decelerating the car
-_DISTRACTED_TIME = 10.
-_DISTRACTED_PRE_TIME_TILL_TERMINAL = 7.
-_DISTRACTED_PROMPT_TIME_TILL_TERMINAL = 5.
+_AWARENESS_TIME = 100.  # 1.6 minutes limit without user touching steering wheels make the car enter a terminal status
+_AWARENESS_PRE_TIME_TILL_TERMINAL = 25.  # a first alert is issued 25s before expiration
+_AWARENESS_PROMPT_TIME_TILL_TERMINAL = 15.  # a second alert is issued 15s before start decelerating the car
+_DISTRACTED_TIME = 11.
+_DISTRACTED_PRE_TIME_TILL_TERMINAL = 8.
+_DISTRACTED_PROMPT_TIME_TILL_TERMINAL = 6.
 
 _FACE_THRESHOLD = 0.4
 _EYE_THRESHOLD = 0.4
@@ -18,8 +19,15 @@ _METRIC_THRESHOLD = 0.4
 _PITCH_POS_ALLOWANCE = 0.04 # 0.08  # rad, to not be too sensitive on positive pitch
 _PITCH_NATURAL_OFFSET = 0.12  # 0.1   # people don't seem to look straight when they drive relaxed, rather a bit up
 _YAW_NATURAL_OFFSET = 0.08  # people don't seem to look straight when they drive relaxed, rather a bit to the right (center of car)
+
 _DISTRACTED_FILTER_TS = 0.25  # 0.6Hz
-_VARIANCE_FILTER_TS = 20.     # 0.008Hz
+
+_POSE_CALIB_MIN_SPEED = 13 # 30 mph
+_POSE_OFFSET_MIN_COUNT = 600 # valid data counts before calibration completes, 1 seg is 600 counts
+_POSE_OFFSET_MAX_COUNT = 3600 # stop deweighting new data after 6 min, aka "short term memory"
+
+_RECOVERY_FACTOR_MAX = 5. # relative to minus step change
+_RECOVERY_FACTOR_MIN = 1.25 # relative to minus step change
 
 MAX_TERMINAL_ALERTS = 3      # not allowed to engage after 3 terminal alerts
 
@@ -36,7 +44,6 @@ def head_orientation_from_descriptor(angles_desc, pos_desc, rpy_calib):
   # the output of these angles are in device frame
   # so from driver's perspective, pitch is up and yaw is right
 
-  # TODO: calibrate based on position
   pitch_prnet = angles_desc[0]
   yaw_prnet = angles_desc[1]
   roll_prnet = angles_desc[2]
@@ -52,72 +59,81 @@ def head_orientation_from_descriptor(angles_desc, pos_desc, rpy_calib):
   # no calib for roll
   pitch -= rpy_calib[1]
   yaw -= rpy_calib[2]
-
   return np.array([roll, pitch, yaw])
 
-
-class _DriverPose():
+class DriverPose():
   def __init__(self):
     self.yaw = 0.
     self.pitch = 0.
     self.roll = 0.
-    self.yaw_offset = 0.
-    self.pitch_offset = 0.
+    self.pitch_offseter = RunningStatFilter(max_trackable=_POSE_OFFSET_MAX_COUNT)
+    self.yaw_offseter = RunningStatFilter(max_trackable=_POSE_OFFSET_MAX_COUNT)
 
-class _DriverBlink():
+class DriverBlink():
   def __init__(self):
     self.left_blink = 0.
     self.right_blink = 0.
 
-
-
-def _monitor_hysteresis(variance_level, monitor_valid_prev):
-  var_thr = 0.63 if monitor_valid_prev else 0.37
-  return variance_level < var_thr
-
 class DriverStatus():
-  def __init__(self, monitor_on=False):
-    self.pose = _DriverPose()
-    self.blink = _DriverBlink()
-    self.monitor_on = monitor_on
-    self.monitor_param_on = monitor_on
-    self.monitor_valid = True   # variance needs to be low
+  def __init__(self):
+    self.pose = DriverPose()
+    self.pose_calibrated = self.pose.pitch_offseter.filtered_stat.n > _POSE_OFFSET_MIN_COUNT and \
+                            self.pose.yaw_offseter.filtered_stat.n > _POSE_OFFSET_MIN_COUNT
+    self.blink = DriverBlink()
     self.awareness = 1.
+    self.awareness_active = 1.
     self.driver_distracted = False
     self.driver_distraction_filter = FirstOrderFilter(0., _DISTRACTED_FILTER_TS, DT_DMON)
-    self.variance_high = False
-    self.variance_filter = FirstOrderFilter(0., _VARIANCE_FILTER_TS, DT_DMON)
-    self.ts_last_check = 0.
     self.face_detected = False
     self.terminal_alert_cnt = 0
     self.step_change = 0.
-    self._set_timers(self.monitor_on)
+    self.active_monitoring_mode = True
+    self.threshold_prompt = _DISTRACTED_PROMPT_TIME_TILL_TERMINAL / _DISTRACTED_TIME
 
-  def _reset_filters(self):
-    self.driver_distraction_filter.x = 0.
-    self.variance_filter.x = 0.
-    self.monitor_valid = True
+    self.is_rhd_region = False
+    self.is_rhd_region_checked = False
+
+    self._set_timers(active_monitoring=True)
 
   def _set_timers(self, active_monitoring):
+    if self.active_monitoring_mode and self.awareness <= self.threshold_prompt:
+      if active_monitoring:
+        self.step_change = DT_CTRL / _DISTRACTED_TIME
+      else:
+        self.step_change = 0.
+      return # no exploit after orange alert
+    elif self.awareness <= 0.:
+      return
+
     if active_monitoring:
       # when falling back from passive mode to active mode, reset awareness to avoid false alert
-      if self.step_change == DT_CTRL / _AWARENESS_TIME:
-        self.awareness = 1.
+      if not self.active_monitoring_mode:
+        self.awareness = self.awareness_active
+
       self.threshold_pre = _DISTRACTED_PRE_TIME_TILL_TERMINAL / _DISTRACTED_TIME
       self.threshold_prompt = _DISTRACTED_PROMPT_TIME_TILL_TERMINAL / _DISTRACTED_TIME
       self.step_change = DT_CTRL / _DISTRACTED_TIME
+      self.active_monitoring_mode = True
     else:
+      if self.active_monitoring_mode:
+        self.awareness_active = self.awareness
+
       self.threshold_pre = _AWARENESS_PRE_TIME_TILL_TERMINAL / _AWARENESS_TIME
       self.threshold_prompt = _AWARENESS_PROMPT_TIME_TILL_TERMINAL / _AWARENESS_TIME
       self.step_change = DT_CTRL / _AWARENESS_TIME
+      self.active_monitoring_mode = False
 
   def _is_driver_distracted(self, pose, blink):
-    # TODO: natural pose calib of each driver
+    if not self.pose_calibrated:
       pitch_error = pose.pitch - _PITCH_NATURAL_OFFSET
       yaw_error = pose.yaw - _YAW_NATURAL_OFFSET
       # add positive pitch allowance
       if pitch_error > 0.:
         pitch_error = max(pitch_error - _PITCH_POS_ALLOWANCE, 0.)
+    else:
+      pitch_error = pose.pitch - self.pose.pitch_offseter.filtered_stat.mean()
+      yaw_error = pose.yaw - self.pose.yaw_offseter.filtered_stat.mean()
+
     pitch_error *= _PITCH_WEIGHT
     pose_metric = np.sqrt(yaw_error**2 + pitch_error**2)
 
@@ -128,76 +144,67 @@ class DriverStatus():
     else:
       return DistractedType.NOT_DISTRACTED
 
-
-  def get_pose(self, driver_monitoring, params, cal_rpy):
+  def get_pose(self, driver_monitoring, cal_rpy, car_speed, op_engaged):
+    # 10 Hz
     if len(driver_monitoring.faceOrientation) == 0 or len(driver_monitoring.facePosition) == 0:
       return
 
     self.pose.roll, self.pose.pitch, self.pose.yaw = head_orientation_from_descriptor(driver_monitoring.faceOrientation, driver_monitoring.facePosition, cal_rpy)
     self.blink.left_blink = driver_monitoring.leftBlinkProb * (driver_monitoring.leftEyeProb>_EYE_THRESHOLD)
     self.blink.right_blink = driver_monitoring.rightBlinkProb * (driver_monitoring.rightEyeProb>_EYE_THRESHOLD)
-    self.face_detected = driver_monitoring.faceProb > _FACE_THRESHOLD
+    self.face_detected = driver_monitoring.faceProb > _FACE_THRESHOLD and not self.is_rhd_region
 
     self.driver_distracted = self._is_driver_distracted(self.pose, self.blink)>0
     # first order filters
     self.driver_distraction_filter.update(self.driver_distracted)
 
-    monitor_param_on_prev = self.monitor_param_on
-
-    # don't check for param too often as it's a kernel call
-    ts = sec_since_boot()
-    if ts - self.ts_last_check > 1.:
-      self.monitor_param_on = params.get("IsDriverMonitoringEnabled") == "1"
-      self.ts_last_check = ts
+    # update offseter
+    # only update when driver is actively driving the car above a certain speed
+    if self.face_detected and car_speed>_POSE_CALIB_MIN_SPEED and not op_engaged:
+      self.pose.pitch_offseter.push_and_update(self.pose.pitch)
+      self.pose.yaw_offseter.push_and_update(self.pose.yaw)
 
-    self.monitor_on = self.monitor_valid and self.monitor_param_on
-    if monitor_param_on_prev != self.monitor_param_on:
-      self._reset_filters()
-    self._set_timers(self.monitor_on and self.face_detected)
+    self.pose_calibrated = self.pose.pitch_offseter.filtered_stat.n > _POSE_OFFSET_MIN_COUNT and \
+                            self.pose.yaw_offseter.filtered_stat.n > _POSE_OFFSET_MIN_COUNT
 
+    self._set_timers(self.face_detected)
 
   def update(self, events, driver_engaged, ctrl_active, standstill):
-    if driver_engaged:
+    if (driver_engaged and self.awareness > 0) or not ctrl_active:
+      # reset only when on disengagement if red reached
       self.awareness = 1.
+      self.awareness_active = 1.
       return events
 
-    driver_engaged |= (self.driver_distraction_filter.x < 0.37 and self.monitor_on)
+    driver_attentive = self.driver_distraction_filter.x < 0.37
     awareness_prev = self.awareness
 
-    if (driver_engaged and self.awareness > 0) or not ctrl_active:
-      # always reset if driver is in control (unless we are in red alert state) or op isn't active
-      self.awareness = min(self.awareness + (2.75*(1.-self.awareness)+1.25)*self.step_change, 1.)
+    if (driver_attentive and self.face_detected and self.awareness > 0):
+      # only restore awareness when paying attention and alert is not red
+      self.awareness = min(self.awareness + ((_RECOVERY_FACTOR_MAX-_RECOVERY_FACTOR_MIN)*(1.-self.awareness)+_RECOVERY_FACTOR_MIN)*self.step_change, 1.)
+      # don't display alert banner when awareness is recovering and has cleared orange
+      if self.awareness > self.threshold_prompt:
+        return events
 
     # should always be counting if distracted unless at standstill and reaching orange
-    if ((not self.monitor_on or (self.monitor_on and not self.face_detected)) or (self.driver_distraction_filter.x > 0.63 and self.driver_distracted and self.face_detected)) and \
+    if (not self.face_detected or (self.driver_distraction_filter.x > 0.63 and self.driver_distracted and self.face_detected)) and \
        not (standstill and self.awareness - self.step_change <= self.threshold_prompt):
       self.awareness = max(self.awareness - self.step_change, -0.1)
 
     alert = None
-    if self.awareness < 0.:
+    if self.awareness <= 0.:
       # terminal red alert: disengagement required
-      alert = 'driverDistracted' if self.monitor_on else 'driverUnresponsive'
-      if awareness_prev >= 0.:
+      alert = 'driverDistracted' if self.active_monitoring_mode else 'driverUnresponsive'
+      if awareness_prev > 0.:
         self.terminal_alert_cnt += 1
     elif self.awareness <= self.threshold_prompt:
       # prompt orange alert
-      alert = 'promptDriverDistracted' if self.monitor_on else 'promptDriverUnresponsive'
+      alert = 'promptDriverDistracted' if self.active_monitoring_mode else 'promptDriverUnresponsive'
     elif self.awareness <= self.threshold_pre:
       # pre green alert
-      alert = 'preDriverDistracted' if self.monitor_on else 'preDriverUnresponsive'
+      alert = 'preDriverDistracted' if self.active_monitoring_mode else 'preDriverUnresponsive'
 
     if alert is not None:
       events.append(create_event(alert, [ET.WARNING]))
 
     return events
-
-
-if __name__ == "__main__":
-  ds = DriverStatus(True)
-  ds.driver_distraction_filter.x = 0.
-  ds.driver_distracted = 1
-  for i in range(10):
-    ds.update([], False, True, False)
-    print(ds.awareness, ds.driver_distracted, ds.driver_distraction_filter.x)
-  ds.update([], True, True, False)
-  print(ds.awareness, ds.driver_distracted, ds.driver_distraction_filter.x)

selfdrive/controls/lib/fcw.py

@@ -45,7 +45,6 @@ class FCWChecker(object):
 
   def update(self, mpc_solution, cur_time, active, v_ego, a_ego, x_lead, v_lead, a_lead, y_lead, vlat_lead, fcw_lead, blinkers):
     mpc_solution_a = list(mpc_solution[0].a_ego)
-    a_target = mpc_solution_a[1]
 
     self.last_min_a = min(mpc_solution_a)
     self.v_lead_max = max(self.v_lead_max, v_lead)
@@ -66,9 +65,8 @@ class FCWChecker(object):
 
       future_fcw_allowed = all(c >= 10 for c in self.counters.values())
       future_fcw = (self.last_min_a < -3.0 or a_delta < a_thr) and future_fcw_allowed
-      current_fcw = a_target < -3.0 and active
 
-      if (future_fcw or current_fcw) and (self.last_fcw_time + 5.0 < cur_time):
+      if future_fcw and (self.last_fcw_time + 5.0 < cur_time):
         self.last_fcw_time = cur_time
         self.last_fcw_a = self.last_min_a
         return True

selfdrive/controls/lib/gps_helpers.py

@@ -0,0 +1,17 @@
+_RHD_REGION_MAP = [ ['AUS', -54.76, -9.23, 112.91, 159.11], \
+                    ['IN1', 6.75, 28.10, 68.17, 97.4], \
+                    ['IN2', 28.09, 35.99, 72.18, 80.87], \
+                    ['IRL', 51.42, 55.38, -10.58, -5.99], \
+                    ['JP1', 32.66, 45.52, 137.27, 146.02], \
+                    ['JP2', 32.79, 37.60, 131.41, 137.28], \
+                    ['JP3', 24.04, 34.78, 122.93, 131.42], \
+                    ['NZ', -52.61, -29.24, 166, 178.84], \
+                    ['SF', -35.14, -22.13, 16.07, 33.21], \
+                    ['UK', 49.9, 60.84, -8.62, 1.77] ]
+
+def is_rhd_region(latitude, longitude):
+  for region in _RHD_REGION_MAP:
+    if region[1] <= latitude <= region[2] and \
+      region[3] <= longitude <= region[4]:
+      return True
+  return False

selfdrive/controls/lib/lane_planner.py

@@ -1,9 +1,19 @@
 from common.numpy_fast import interp
 import numpy as np
-from selfdrive.controls.lib.latcontrol_helpers import model_polyfit, compute_path_pinv
 
 CAMERA_OFFSET = 0.06  # m from center car to camera
 
+def compute_path_pinv(l=50):
+  deg = 3
+  x = np.arange(l*1.0)
+  X = np.vstack(tuple(x**n for n in range(deg, -1, -1))).T
+  pinv = np.linalg.pinv(X)
+  return pinv
+
+
+def model_polyfit(points, path_pinv):
+  return np.dot(path_pinv, [float(x) for x in points])
+
 
 def calc_d_poly(l_poly, r_poly, p_poly, l_prob, r_prob, lane_width):
   # This will improve behaviour when lanes suddenly widen
@@ -16,7 +26,7 @@ def calc_d_poly(l_poly, r_poly, p_poly, l_prob, r_prob, lane_width):
   path_from_right_lane = r_poly.copy()
   path_from_right_lane[3] += lane_width / 2.0
 
-  lr_prob = l_prob + r_prob - l_prob * r_prob
+  lr_prob = l_prob * r_prob
 
   d_poly_lane = (l_prob * path_from_left_lane + r_prob * path_from_right_lane) / (l_prob + r_prob + 0.0001)
   return lr_prob * d_poly_lane + (1.0 - lr_prob) * p_poly
@@ -35,7 +45,6 @@ class LanePlanner(object):
 
     self.l_prob = 0.
     self.r_prob = 0.
-    self.lr_prob = 0.
 
     self._path_pinv = compute_path_pinv()
     self.x_points = np.arange(50)
@@ -57,8 +66,6 @@ class LanePlanner(object):
     self.l_poly[3] += CAMERA_OFFSET
     self.r_poly[3] += CAMERA_OFFSET
 
-    self.lr_prob = self.l_prob + self.r_prob - self.l_prob * self.r_prob
-
     # Find current lanewidth
     self.lane_width_certainty += 0.05 * (self.l_prob * self.r_prob - self.lane_width_certainty)
     current_lane_width = abs(self.l_poly[3] - self.r_poly[3])

selfdrive/controls/lib/latcontrol_helpers.py

@@ -1,62 +0,0 @@
-import numpy as np
-import math
-from common.numpy_fast import interp
-
-_K_CURV_V = [1., 0.6]
-_K_CURV_BP = [0., 0.002]
-
-# lane width http://safety.fhwa.dot.gov/geometric/pubs/mitigationstrategies/chapter3/3_lanewidth.cfm
-_LANE_WIDTH_V = [3., 3.8]
-
-# break points of speed
-_LANE_WIDTH_BP = [0., 31.]
-
-
-def calc_d_lookahead(v_ego, d_poly):
-  # this function computes how far too look for lateral control
-  # howfar we look ahead is function of speed and how much curvy is the path
-  offset_lookahead = 1.
-  k_lookahead = 7.
-  # integrate abs value of second derivative of poly to get a measure of path curvature
-  pts_len = 50.  # m
-  if len(d_poly) > 0:
-    pts = np.polyval([6 * d_poly[0], 2 * d_poly[1]], np.arange(0, pts_len))
-  else:
-    pts = 0.
-  curv = np.sum(np.abs(pts)) / pts_len
-
-  k_curv = interp(curv, _K_CURV_BP, _K_CURV_V)
-
-  # sqrt on speed is needed to keep, for a given curvature, the y_des
-  # proportional to speed. Indeed, y_des is prop to d_lookahead^2
-  # 36m at 25m/s
-  d_lookahead = offset_lookahead + math.sqrt(max(v_ego, 0)) * k_lookahead * k_curv
-  return d_lookahead
-
-
-def calc_lookahead_offset(v_ego, angle_steers, d_lookahead, VM, angle_offset):
-  # this function returns the lateral offset given the steering angle, speed and the lookahead distance
-  sa = math.radians(angle_steers - angle_offset)
-  curvature = VM.calc_curvature(sa, v_ego)
-  # clip is to avoid arcsin NaNs due to too sharp turns
-  y_actual = d_lookahead * np.tan(np.arcsin(np.clip(d_lookahead * curvature, -0.999, 0.999)) / 2.)
-  return y_actual, curvature
-
-
-def calc_desired_steer_angle(v_ego, y_des, d_lookahead, VM, angle_offset):
-  # inverse of the above function
-  curvature = np.sin(np.arctan(y_des / d_lookahead) * 2.) / d_lookahead
-  steer_des = math.degrees(VM.get_steer_from_curvature(curvature, v_ego)) + angle_offset
-  return steer_des, curvature
-
-
-def compute_path_pinv(l=50):
-  deg = 3
-  x = np.arange(l*1.0)
-  X = np.vstack(tuple(x**n for n in range(deg, -1, -1))).T
-  pinv = np.linalg.pinv(X)
-  return pinv
-
-
-def model_polyfit(points, path_pinv):
-  return np.dot(path_pinv, [float(x) for x in points])

selfdrive/controls/lib/latcontrol_lqr.py

@@ -22,6 +22,7 @@ class LatControlLQR(object):
     self.i_unwind_rate = 0.3 / rate
     self.i_rate = 1.0 / rate
 
+
     self.reset()
 
   def reset(self):
@@ -31,6 +32,7 @@ class LatControlLQR(object):
   def update(self, active, v_ego, angle_steers, angle_steers_rate, eps_torque, steer_override, CP, VM, path_plan):
     lqr_log = log.ControlsState.LateralLQRState.new_message()
 
+    steers_max = get_steer_max(CP, v_ego)
     torque_scale = (0.45 + v_ego / 60.0)**2  # Scale actuator model with speed
 
     # Subtract offset. Zero angle should correspond to zero torque
@@ -44,29 +46,32 @@ class LatControlLQR(object):
 
     if v_ego < 0.3 or not active:
       lqr_log.active = False
+      lqr_output = 0.
       self.reset()
     else:
       lqr_log.active = True
 
       # LQR
       u_lqr = float(self.angle_steers_des / self.dc_gain - self.K.dot(self.x_hat))
+      lqr_output = torque_scale * u_lqr / self.scale
 
       # Integrator
       if steer_override:
         self.i_lqr -= self.i_unwind_rate * float(np.sign(self.i_lqr))
       else:
-        self.i_lqr += self.ki * self.i_rate * (self.angle_steers_des - angle_steers_k)
+        error = self.angle_steers_des - angle_steers_k
+        i = self.i_lqr + self.ki * self.i_rate * error
+        control = lqr_output + i
 
-      lqr_output = torque_scale * u_lqr / self.scale
-      self.i_lqr = clip(self.i_lqr, -1.0 - lqr_output, 1.0 - lqr_output) # (LQR + I) has to be between -1 and 1
+        if ((error >= 0 and (control <= steers_max or i < 0.0)) or \
+            (error <= 0 and (control >= -steers_max or i > 0.0))):
+          self.i_lqr = i
 
       self.output_steer = lqr_output + self.i_lqr
-
-      # Clip output
-      steers_max = get_steer_max(CP, v_ego)
       self.output_steer = clip(self.output_steer, -steers_max, steers_max)
 
     lqr_log.steerAngle = angle_steers_k + path_plan.angleOffset
     lqr_log.i = self.i_lqr
     lqr_log.output = self.output_steer
+    lqr_log.lqrOutput = lqr_output
     return self.output_steer, float(self.angle_steers_des), lqr_log

selfdrive/controls/lib/long_mpc.py

@@ -12,8 +12,7 @@ LOG_MPC = os.environ.get('LOG_MPC', False)
 
 
 class LongitudinalMpc(object):
-  def __init__(self, mpc_id, live_longitudinal_mpc):
-    self.live_longitudinal_mpc = live_longitudinal_mpc
+  def __init__(self, mpc_id):
     self.mpc_id = mpc_id
 
     self.setup_mpc()
@@ -27,7 +26,7 @@ class LongitudinalMpc(object):
 
     self.last_cloudlog_t = 0.0
 
-  def send_mpc_solution(self, qp_iterations, calculation_time):
+  def send_mpc_solution(self, pm, qp_iterations, calculation_time):
     qp_iterations = max(0, qp_iterations)
     dat = messaging.new_message()
     dat.init('liveLongitudinalMpc')
@@ -41,7 +40,7 @@ class LongitudinalMpc(object):
     dat.liveLongitudinalMpc.qpIterations = qp_iterations
     dat.liveLongitudinalMpc.mpcId = self.mpc_id
     dat.liveLongitudinalMpc.calculationTime = calculation_time
-    self.live_longitudinal_mpc.send(dat.to_bytes())
+    pm.send('liveLongitudinalMpc', dat)
 
   def setup_mpc(self):
     ffi, self.libmpc = libmpc_py.get_libmpc(self.mpc_id)
@@ -58,7 +57,7 @@ class LongitudinalMpc(object):
     self.cur_state[0].v_ego = v
     self.cur_state[0].a_ego = a
 
-  def update(self, CS, lead, v_cruise_setpoint):
+  def update(self, pm, CS, lead, v_cruise_setpoint):
     v_ego = CS.vEgo
 
     # Setup current mpc state
@@ -97,7 +96,7 @@ class LongitudinalMpc(object):
     duration = int((sec_since_boot() - t) * 1e9)
 
     if LOG_MPC:
-      self.send_mpc_solution(n_its, duration)
+      self.send_mpc_solution(pm, n_its, duration)
 
     # Get solution. MPC timestep is 0.2 s, so interpolation to 0.05 s is needed
     self.v_mpc = self.mpc_solution[0].v_ego[1]

selfdrive/controls/lib/pathplanner.py

@@ -4,7 +4,6 @@ import numpy as np
 
 # from common.numpy_fast import clip
 from common.realtime import sec_since_boot
-from selfdrive.services import service_list
 from selfdrive.swaglog import cloudlog
 from selfdrive.controls.lib.lateral_mpc import libmpc_py
 from selfdrive.controls.lib.drive_helpers import MPC_COST_LAT
@@ -26,9 +25,6 @@ class PathPlanner(object):
 
     self.last_cloudlog_t = 0
 
-    self.plan = messaging.pub_sock(service_list['pathPlan'].port)
-    self.livempc = messaging.pub_sock(service_list['liveMpc'].port)
-
     self.setup_mpc(CP.steerRateCost)
     self.solution_invalid_cnt = 0
     self.path_offset_i = 0.0
@@ -49,13 +45,12 @@ class PathPlanner(object):
     self.angle_steers_des_prev = 0.0
     self.angle_steers_des_time = 0.0
 
-  def update(self, sm, CP, VM):
+  def update(self, sm, pm, CP, VM):
     v_ego = sm['carState'].vEgo
     angle_steers = sm['carState'].steeringAngle
     active = sm['controlsState'].active
 
-    angle_offset_average = sm['liveParameters'].angleOffsetAverage
-    angle_offset_bias = sm['controlsState'].angleModelBias + angle_offset_average
+    angle_offset = sm['liveParameters'].angleOffset
 
     self.LP.update(v_ego, sm['model'])
 
@@ -73,7 +68,7 @@ class PathPlanner(object):
     #   self.path_offset_i = 0.0
 
     # account for actuation delay
-    self.cur_state = calc_states_after_delay(self.cur_state, v_ego, angle_steers - angle_offset_average, curvature_factor, VM.sR, CP.steerActuatorDelay)
+    self.cur_state = calc_states_after_delay(self.cur_state, v_ego, angle_steers - angle_offset, curvature_factor, VM.sR, CP.steerActuatorDelay)
 
     v_ego_mpc = max(v_ego, 5.0)  # avoid mpc roughness due to low speed
     self.libmpc.run_mpc(self.cur_state, self.mpc_solution,
@@ -85,19 +80,19 @@ class PathPlanner(object):
       delta_desired = self.mpc_solution[0].delta[1]
       rate_desired = math.degrees(self.mpc_solution[0].rate[0] * VM.sR)
     else:
-      delta_desired = math.radians(angle_steers - angle_offset_bias) / VM.sR
+      delta_desired = math.radians(angle_steers - angle_offset) / VM.sR
       rate_desired = 0.0
 
     self.cur_state[0].delta = delta_desired
 
-    self.angle_steers_des_mpc = float(math.degrees(delta_desired * VM.sR) + angle_offset_bias)
+    self.angle_steers_des_mpc = float(math.degrees(delta_desired * VM.sR) + angle_offset)
 
     #  Check for infeasable MPC solution
     mpc_nans = np.any(np.isnan(list(self.mpc_solution[0].delta)))
     t = sec_since_boot()
     if mpc_nans:
       self.libmpc.init(MPC_COST_LAT.PATH, MPC_COST_LAT.LANE, MPC_COST_LAT.HEADING, CP.steerRateCost)
-      self.cur_state[0].delta = math.radians(angle_steers - angle_offset_bias) / VM.sR
+      self.cur_state[0].delta = math.radians(angle_steers - angle_offset) / VM.sR
 
       if t > self.last_cloudlog_t + 5.0:
         self.last_cloudlog_t = t
@@ -121,13 +116,13 @@ class PathPlanner(object):
 
     plan_send.pathPlan.angleSteers = float(self.angle_steers_des_mpc)
     plan_send.pathPlan.rateSteers = float(rate_desired)
-    plan_send.pathPlan.angleOffset = float(self.path_offset_i)
+    plan_send.pathPlan.angleOffset = float(sm['liveParameters'].angleOffsetAverage)
     plan_send.pathPlan.mpcSolutionValid = bool(plan_solution_valid)
     plan_send.pathPlan.paramsValid = bool(sm['liveParameters'].valid)
     plan_send.pathPlan.sensorValid = bool(sm['liveParameters'].sensorValid)
     plan_send.pathPlan.posenetValid = bool(sm['liveParameters'].posenetValid)
 
-    self.plan.send(plan_send.to_bytes())
+    pm.send('pathPlan', plan_send)
 
     if LOG_MPC:
       dat = messaging.new_message()
@@ -137,4 +132,4 @@ class PathPlanner(object):
       dat.liveMpc.psi = list(self.mpc_solution[0].psi)
       dat.liveMpc.delta = list(self.mpc_solution[0].delta)
       dat.liveMpc.cost = self.mpc_solution[0].cost
-      self.livempc.send(dat.to_bytes())
+      pm.send('liveMpc', dat)

selfdrive/controls/lib/planner.py

@@ -9,7 +9,6 @@ from cereal import car
 from common.realtime import sec_since_boot, DT_PLAN
 from selfdrive.swaglog import cloudlog
 from selfdrive.config import Conversions as CV
-from selfdrive.services import service_list
 from selfdrive.controls.lib.speed_smoother import speed_smoother
 from selfdrive.controls.lib.longcontrol import LongCtrlState, MIN_CAN_SPEED
 from selfdrive.controls.lib.fcw import FCWChecker
@@ -71,14 +70,11 @@ def limit_accel_in_turns(v_ego, angle_steers, a_target, CP):
 
 
 class Planner(object):
-  def __init__(self, CP, fcw_enabled):
+  def __init__(self, CP):
     self.CP = CP
 
-    self.plan = messaging.pub_sock(service_list['plan'].port)
-    self.live_longitudinal_mpc = messaging.pub_sock(service_list['liveLongitudinalMpc'].port)
-
-    self.mpc1 = LongitudinalMpc(1, self.live_longitudinal_mpc)
-    self.mpc2 = LongitudinalMpc(2, self.live_longitudinal_mpc)
+    self.mpc1 = LongitudinalMpc(1)
+    self.mpc2 = LongitudinalMpc(2)
 
     self.v_acc_start = 0.0
     self.a_acc_start = 0.0
@@ -93,7 +89,6 @@ class Planner(object):
 
     self.longitudinalPlanSource = 'cruise'
     self.fcw_checker = FCWChecker()
-    self.fcw_enabled = fcw_enabled
     self.path_x = np.arange(192)
 
     self.params = Params()
@@ -125,7 +120,7 @@ class Planner(object):
 
     self.v_acc_future = min([self.mpc1.v_mpc_future, self.mpc2.v_mpc_future, v_cruise_setpoint])
 
-  def update(self, sm, CP, VM, PP):
+  def update(self, sm, pm, CP, VM, PP):
     """Gets called when new radarState is available"""
     cur_time = sec_since_boot()
     v_ego = sm['carState'].vEgo
@@ -199,8 +194,8 @@ class Planner(object):
     self.mpc1.set_cur_state(self.v_acc_start, self.a_acc_start)
     self.mpc2.set_cur_state(self.v_acc_start, self.a_acc_start)
 
-    self.mpc1.update(sm['carState'], lead_1, v_cruise_setpoint)
-    self.mpc2.update(sm['carState'], lead_2, v_cruise_setpoint)
+    self.mpc1.update(pm, sm['carState'], lead_1, v_cruise_setpoint)
+    self.mpc2.update(pm, sm['carState'], lead_2, v_cruise_setpoint)
 
     self.choose_solution(v_cruise_setpoint, enabled)
 
@@ -251,10 +246,9 @@ class Planner(object):
     plan_send.plan.processingDelay = (plan_send.logMonoTime / 1e9) - sm.rcv_time['radarState']
 
     # Send out fcw
-    fcw = fcw and (self.fcw_enabled or long_control_state != LongCtrlState.off)
     plan_send.plan.fcw = fcw
 
-    self.plan.send(plan_send.to_bytes())
+    pm.send('plan', plan_send)
 
     # Interpolate 0.05 seconds and save as starting point for next iteration
     a_acc_sol = self.a_acc_start + (DT_PLAN / LON_MPC_STEP) * (self.a_acc - self.a_acc_start)

selfdrive/controls/lib/radar_helpers.py

@@ -26,6 +26,7 @@ class Track(object):
   def __init__(self):
     self.ekf = None
     self.cnt = 0
+    self.aLeadTau = _LEAD_ACCEL_TAU
 
   def update(self, d_rel, y_rel, v_rel, v_ego_t_aligned, measured):
     # relative values, copy
@@ -123,7 +124,7 @@ class Cluster(object):
 
   @property
   def measured(self):
-    return any([t.measured for t in self.tracks])
+    return any(t.measured for t in self.tracks)
 
   def get_RadarState(self, model_prob=0.0):
     return {

selfdrive/controls/plannerd.py

@@ -11,28 +11,27 @@ from selfdrive.controls.lib.pathplanner import PathPlanner
 import selfdrive.messaging as messaging
 
 
-def plannerd_thread():
+def plannerd_thread(sm=None, pm=None):
   gc.disable()
 
   # start the loop
   set_realtime_priority(2)
 
-  params = Params()
-
-  # Get FCW toggle from settings
-  fcw_enabled = params.get("IsFcwEnabled") == "1"
-
   cloudlog.info("plannerd is waiting for CarParams")
   CP = car.CarParams.from_bytes(Params().get("CarParams", block=True))
   cloudlog.info("plannerd got CarParams: %s", CP.carName)
 
-  PL = Planner(CP, fcw_enabled)
+  PL = Planner(CP)
   PP = PathPlanner(CP)
 
   VM = VehicleModel(CP)
 
+  if sm is None:
     sm = messaging.SubMaster(['carState', 'controlsState', 'radarState', 'model', 'liveParameters'])
 
+  if pm is None:
+    pm = messaging.PubMaster(['plan', 'liveLongitudinalMpc', 'pathPlan', 'liveMpc'])
+
   sm['liveParameters'].valid = True
   sm['liveParameters'].sensorValid = True
   sm['liveParameters'].steerRatio = CP.steerRatio
@@ -42,13 +41,13 @@ def plannerd_thread():
     sm.update()
 
     if sm.updated['model']:
-      PP.update(sm, CP, VM)
+      PP.update(sm, pm, CP, VM)
     if sm.updated['radarState']:
-      PL.update(sm, CP, VM, PP)
+      PL.update(sm, pm, CP, VM, PP)
 
 
-def main(gctx=None):
-  plannerd_thread()
+def main(sm=None, pm=None):
+  plannerd_thread(sm, pm)
 
 
 if __name__ == "__main__":

selfdrive/controls/radard.py

@@ -173,7 +173,7 @@ class RadarD(object):
 
 
 # fuses camera and radar data for best lead detection
-def radard_thread(gctx=None):
+def radard_thread(sm=None, pm=None, can_sock=None):
   set_realtime_priority(2)
 
   # wait for stats about the car to come in from controls
@@ -186,14 +186,17 @@ def radard_thread(gctx=None):
   cloudlog.info("radard is importing %s", CP.carName)
   RadarInterface = importlib.import_module('selfdrive.car.%s.radar_interface' % CP.carName).RadarInterface
 
+  if can_sock is None:
     can_sock = messaging.sub_sock(service_list['can'].port)
-  sm = messaging.SubMaster(['model', 'controlsState', 'liveParameters'])
 
-  RI = RadarInterface(CP)
+  if sm is None:
+    sm = messaging.SubMaster(['model', 'controlsState', 'liveParameters'])
 
   # *** publish radarState and liveTracks
-  radarState = messaging.pub_sock(service_list['radarState'].port)
-  liveTracks = messaging.pub_sock(service_list['liveTracks'].port)
+  if pm is None:
+    pm = messaging.PubMaster(['radarState', 'liveTracks'])
+
+  RI = RadarInterface(CP)
 
   rk = Ratekeeper(rate, print_delay_threshold=None)
   RD = RadarD(mocked)
@@ -212,7 +215,7 @@ def radard_thread(gctx=None):
     dat = RD.update(rk.frame, RI.delay, sm, rr, has_radar)
     dat.radarState.cumLagMs = -rk.remaining*1000.
 
-    radarState.send(dat.to_bytes())
+    pm.send('radarState', dat)
 
     # *** publish tracks for UI debugging (keep last) ***
     tracks = RD.tracks
@@ -226,13 +229,13 @@ def radard_thread(gctx=None):
         "yRel": float(tracks[ids].yRel),
         "vRel": float(tracks[ids].vRel),
       }
-    liveTracks.send(dat.to_bytes())
+    pm.send('liveTracks', dat)
 
     rk.monitor_time()
 
 
-def main(gctx=None):
-  radard_thread(gctx)
+def main(sm=None, pm=None, can_sock=None):
+  radard_thread(sm, pm, can_sock)
 
 
 if __name__ == "__main__":

selfdrive/controls/tests/test_following_distance.py

@@ -15,8 +15,8 @@ def RW(v_ego, v_l):
   return (v_ego * TR - (v_l - v_ego) * TR + v_ego * v_ego / (2 * G) - v_l * v_l / (2 * G))
 
 
-class FakeSocket(object):
-    def send(self, data):
+class FakePubMaster(object):
+  def send(self, s, data):
     assert data
 
 
@@ -32,7 +32,8 @@ def run_following_distance_simulation(v_lead, t_end=200.0):
 
   v_cruise_setpoint = v_lead + 10.
 
-  mpc = LongitudinalMpc(1, FakeSocket())
+  pm = FakePubMaster()
+  mpc = LongitudinalMpc(1)
 
   first = True
   while t < t_end:
@@ -61,8 +62,8 @@ def run_following_distance_simulation(v_lead, t_end=200.0):
     mpc.set_cur_state(v_ego, a_ego)
     if first:  # Make sure MPC is converged on first timestep
       for _ in range(20):
-        mpc.update(CS.carState, lead, v_cruise_setpoint)
-    mpc.update(CS.carState, lead, v_cruise_setpoint)
+        mpc.update(pm, CS.carState, lead, v_cruise_setpoint)
+    mpc.update(pm, CS.carState, lead, v_cruise_setpoint)
 
     # Choose slowest of two solutions
     if v_cruise < mpc.v_mpc:

selfdrive/controls/tests/test_monitoring.py

@@ -0,0 +1,193 @@
+import unittest
+import numpy as np
+from common.realtime import DT_CTRL, DT_DMON
+from selfdrive.controls.lib.driver_monitor import DriverStatus, MAX_TERMINAL_ALERTS, \
+                                  _AWARENESS_TIME, _AWARENESS_PRE_TIME_TILL_TERMINAL, \
+                                  _AWARENESS_PROMPT_TIME_TILL_TERMINAL, _DISTRACTED_TIME, \
+                                  _DISTRACTED_PRE_TIME_TILL_TERMINAL, _DISTRACTED_PROMPT_TIME_TILL_TERMINAL
+from selfdrive.controls.lib.gps_helpers import is_rhd_region
+
+_TEST_TIMESPAN = 120 # seconds
+_DISTRACTED_SECONDS_TO_ORANGE = _DISTRACTED_TIME - _DISTRACTED_PROMPT_TIME_TILL_TERMINAL + 1
+_DISTRACTED_SECONDS_TO_RED = _DISTRACTED_TIME + 1
+_INVISIBLE_SECONDS_TO_ORANGE = _AWARENESS_TIME - _AWARENESS_PROMPT_TIME_TILL_TERMINAL + 1
+_INVISIBLE_SECONDS_TO_RED = _AWARENESS_TIME + 1
+
+class fake_DM_msg():
+  def __init__(self, is_face_detected, is_distracted=False):
+    self.faceOrientation = [0.,0.,0.]
+    self.facePosition = [0.,0.]
+    self.faceProb = 1. * is_face_detected
+    self.leftEyeProb = 1.
+    self.rightEyeProb = 1.
+    self.leftBlinkProb = 1. * is_distracted
+    self.rightBlinkProb = 1. * is_distracted
+
+
+# driver state from neural net, 10Hz
+msg_NO_FACE_DETECTED = fake_DM_msg(is_face_detected=False)
+msg_ATTENTIVE = fake_DM_msg(is_face_detected=True)
+msg_DISTRACTED = fake_DM_msg(is_face_detected=True, is_distracted=True)
+
+# driver interaction with car
+car_interaction_DETECTED = True
+car_interaction_NOT_DETECTED = False
+
+# openpilot state
+openpilot_ENGAGED = True
+openpilot_NOT_ENGAGED = False
+
+# car standstill state
+car_STANDSTILL = True
+car_NOT_STANDSTILL = False
+
+# some common state vectors
+always_no_face = [msg_NO_FACE_DETECTED] * int(_TEST_TIMESPAN/DT_DMON)
+always_attentive = [msg_ATTENTIVE] * int(_TEST_TIMESPAN/DT_DMON)
+always_distracted = [msg_DISTRACTED] * int(_TEST_TIMESPAN/DT_DMON)
+always_true = [True] * int(_TEST_TIMESPAN/DT_DMON)
+always_false = [False] * int(_TEST_TIMESPAN/DT_DMON)
+
+def run_DState_seq(driver_state_msgs, driver_car_interaction, openpilot_status, car_standstill_status):
+  # inputs are all 10Hz
+  DS = DriverStatus()
+  events_from_DM = []
+  for idx in range(len(driver_state_msgs)):
+    DS.get_pose(driver_state_msgs[idx], [0,0,0], 0, openpilot_status[idx])
+                # cal_rpy and car_speed don't matter here
+
+    # to match frequency of controlsd (100Hz)
+    for _ in range(int(DT_DMON/DT_CTRL)):
+      event_per_state = DS.update([], driver_car_interaction[idx], openpilot_status[idx], car_standstill_status[idx])
+    events_from_DM.append(event_per_state) # evaluate events at 10Hz for tests
+
+  assert len(events_from_DM)==len(driver_state_msgs), 'somethings wrong'
+  return events_from_DM
+
+class TestMonitoring(unittest.TestCase):
+  # -1. rhd parser sanity check
+  def test_rhd_parser(self):
+    cities = [[32.7, -117.1, 0],\
+              [51.5, 0.129, 1],\
+              [35.7, 139.7, 1],\
+              [-37.8, 144.9, 1],\
+              [32.1, 41.74, 0],\
+              [55.7, 12.69, 0]]
+    result = []
+    for city in cities:
+      result.append(int(is_rhd_region(city[0],city[1])))
+    self.assertEqual(result,[int(city[2]) for city in cities])
+
+  # 0. op engaged, driver is doing fine all the time
+  def test_fully_aware_driver(self):
+    events_output = run_DState_seq(always_attentive, always_false, always_true, always_false)
+    self.assertTrue(np.sum([len(event) for event in events_output])==0)
+
+  # 1. op engaged, driver is distracted and does nothing
+  def test_fully_distracted_driver(self):
+    events_output = run_DState_seq(always_distracted, always_false, always_true, always_false)
+    self.assertTrue(len(events_output[int((_DISTRACTED_TIME-_DISTRACTED_PRE_TIME_TILL_TERMINAL)/2/DT_DMON)])==0)
+    self.assertEqual(events_output[int((_DISTRACTED_TIME-_DISTRACTED_PRE_TIME_TILL_TERMINAL+\
+                      ((_DISTRACTED_PRE_TIME_TILL_TERMINAL-_DISTRACTED_PROMPT_TIME_TILL_TERMINAL)/2))/DT_DMON)][0].name, 'preDriverDistracted')
+    self.assertEqual(events_output[int((_DISTRACTED_TIME-_DISTRACTED_PROMPT_TIME_TILL_TERMINAL+\
+                      ((_DISTRACTED_PROMPT_TIME_TILL_TERMINAL)/2))/DT_DMON)][0].name, 'promptDriverDistracted')
+    self.assertEqual(events_output[int((_DISTRACTED_TIME+\
+                      ((_TEST_TIMESPAN-10-_DISTRACTED_TIME)/2))/DT_DMON)][0].name, 'driverDistracted')
+
+  # 2. op engaged, no face detected the whole time, no action
+  def test_fully_invisible_driver(self):
+    events_output = run_DState_seq(always_no_face, always_false, always_true, always_false)
+    self.assertTrue(len(events_output[int((_AWARENESS_TIME-_AWARENESS_PRE_TIME_TILL_TERMINAL)/2/DT_DMON)])==0)
+    self.assertEqual(events_output[int((_AWARENESS_TIME-_AWARENESS_PRE_TIME_TILL_TERMINAL+\
+                      ((_AWARENESS_PRE_TIME_TILL_TERMINAL-_AWARENESS_PROMPT_TIME_TILL_TERMINAL)/2))/DT_DMON)][0].name, 'preDriverUnresponsive')
+    self.assertEqual(events_output[int((_AWARENESS_TIME-_AWARENESS_PROMPT_TIME_TILL_TERMINAL+\
+                      ((_AWARENESS_PROMPT_TIME_TILL_TERMINAL)/2))/DT_DMON)][0].name, 'promptDriverUnresponsive')
+    self.assertEqual(events_output[int((_AWARENESS_TIME+\
+                      ((_TEST_TIMESPAN-10-_AWARENESS_TIME)/2))/DT_DMON)][0].name, 'driverUnresponsive')
+
+  # 3. op engaged, down to orange, driver pays attention, back to normal; then down to orange, driver touches wheel
+  #  - should have short orange recovery time and no green afterwards; should recover rightaway on wheel touch
+  def test_normal_driver(self):
+    ds_vector = [msg_DISTRACTED] * int(_DISTRACTED_SECONDS_TO_ORANGE/DT_DMON) + \
+                [msg_ATTENTIVE] * int(_DISTRACTED_SECONDS_TO_ORANGE/DT_DMON) + \
+                [msg_DISTRACTED] * (int(_TEST_TIMESPAN/DT_DMON)-int(_DISTRACTED_SECONDS_TO_ORANGE*2/DT_DMON))
+    interaction_vector = [car_interaction_NOT_DETECTED] * int(_DISTRACTED_SECONDS_TO_ORANGE*3/DT_DMON) + \
+                        [car_interaction_DETECTED] * (int(_TEST_TIMESPAN/DT_DMON)-int(_DISTRACTED_SECONDS_TO_ORANGE*3/DT_DMON))
+    events_output = run_DState_seq(ds_vector, interaction_vector, always_true, always_false)
+    self.assertTrue(len(events_output[int(_DISTRACTED_SECONDS_TO_ORANGE*0.5/DT_DMON)])==0)
+    self.assertEqual(events_output[int((_DISTRACTED_SECONDS_TO_ORANGE-0.1)/DT_DMON)][0].name, 'promptDriverDistracted')
+    self.assertTrue(len(events_output[int(_DISTRACTED_SECONDS_TO_ORANGE*1.5/DT_DMON)])==0)
+    self.assertEqual(events_output[int((_DISTRACTED_SECONDS_TO_ORANGE*3-0.1)/DT_DMON)][0].name, 'promptDriverDistracted')
+    self.assertTrue(len(events_output[int((_DISTRACTED_SECONDS_TO_ORANGE*3+0.1)/DT_DMON)])==0)
+
+  # 4. op engaged, down to orange, driver dodges camera, then comes back still distracted, down to red, \
+  #                          driver dodges, and then touches wheel to no avail, disengages and reengages
+  #  - orange/red alert should remain after disappearance, and only disengaging clears red
+  def test_biggest_comma_fan(self):
+    _invisible_time = 2 # seconds
+    ds_vector = always_distracted[:]
+    interaction_vector = always_false[:]
+    op_vector = always_true[:]
+    ds_vector[int(_DISTRACTED_SECONDS_TO_ORANGE/DT_DMON):int((_DISTRACTED_SECONDS_TO_ORANGE+_invisible_time)/DT_DMON)] = [msg_NO_FACE_DETECTED] * int(_invisible_time/DT_DMON)
+    ds_vector[int((_DISTRACTED_SECONDS_TO_RED+_invisible_time)/DT_DMON):int((_DISTRACTED_SECONDS_TO_RED+2*_invisible_time)/DT_DMON)] = [msg_NO_FACE_DETECTED] * int(_invisible_time/DT_DMON)
+    interaction_vector[int((_DISTRACTED_SECONDS_TO_RED+2*_invisible_time+0.5)/DT_DMON):int((_DISTRACTED_SECONDS_TO_RED+2*_invisible_time+1.5)/DT_DMON)] = [True] * int(1/DT_DMON)
+    op_vector[int((_DISTRACTED_SECONDS_TO_RED+2*_invisible_time+2.5)/DT_DMON):int((_DISTRACTED_SECONDS_TO_RED+2*_invisible_time+3)/DT_DMON)] = [False] * int(0.5/DT_DMON)
+    events_output = run_DState_seq(ds_vector, interaction_vector, op_vector, always_false)
+    self.assertEqual(events_output[int((_DISTRACTED_SECONDS_TO_ORANGE+0.5*_invisible_time)/DT_DMON)][0].name, 'promptDriverDistracted')
+    self.assertEqual(events_output[int((_DISTRACTED_SECONDS_TO_RED+1.5*_invisible_time)/DT_DMON)][0].name, 'driverDistracted')
+    self.assertEqual(events_output[int((_DISTRACTED_SECONDS_TO_RED+2*_invisible_time+1.5)/DT_DMON)][0].name, 'driverDistracted')
+    self.assertTrue(len(events_output[int((_DISTRACTED_SECONDS_TO_RED+2*_invisible_time+3.5)/DT_DMON)])==0)
+
+  # 5. op engaged, invisible driver, down to orange, driver appears; then down to orange again, driver touches wheel
+  #  - both actions should clear the alert
+  def test_sometimes_transparent_commuter(self):
+    _visible_time = 2 # seconds
+    ds_vector = always_no_face[:]*2
+    interaction_vector = always_false[:]*2
+    ds_vector[int(_INVISIBLE_SECONDS_TO_ORANGE/DT_DMON):int((_INVISIBLE_SECONDS_TO_ORANGE+_visible_time)/DT_DMON)] = [msg_ATTENTIVE] * int(_visible_time/DT_DMON)
+    interaction_vector[int((2*_INVISIBLE_SECONDS_TO_ORANGE+_visible_time)/DT_DMON):int((2*_INVISIBLE_SECONDS_TO_ORANGE+_visible_time+1)/DT_DMON)] = [True] * int(1/DT_DMON)
+    events_output = run_DState_seq(ds_vector, interaction_vector, 2*always_true, 2*always_false)
+    self.assertTrue(len(events_output[int(_INVISIBLE_SECONDS_TO_ORANGE*0.5/DT_DMON)])==0)
+    self.assertEqual(events_output[int((_INVISIBLE_SECONDS_TO_ORANGE-0.1)/DT_DMON)][0].name, 'promptDriverUnresponsive')
+    self.assertTrue(len(events_output[int((_INVISIBLE_SECONDS_TO_ORANGE*1.5+_visible_time)/DT_DMON)])==0)
+    self.assertEqual(events_output[int((_INVISIBLE_SECONDS_TO_ORANGE*2+_visible_time-0.5)/DT_DMON)][0].name, 'promptDriverUnresponsive')
+    self.assertTrue(len(events_output[int((_INVISIBLE_SECONDS_TO_ORANGE*2+_visible_time+0.1)/DT_DMON)])==0)
+
+  # 6. op engaged, invisible driver, down to red, driver appears and then touches wheel, then disengages/reengages
+  #  - only disengage will clear the alert
+  def test_last_second_responder(self):
+    _visible_time = 2 # seconds
+    ds_vector = always_no_face[:]
+    interaction_vector = always_false[:]
+    op_vector = always_true[:]
+    ds_vector[int(_INVISIBLE_SECONDS_TO_RED/DT_DMON):int((_INVISIBLE_SECONDS_TO_RED+_visible_time)/DT_DMON)] = [msg_ATTENTIVE] * int(_visible_time/DT_DMON)
+    interaction_vector[int((_INVISIBLE_SECONDS_TO_RED+_visible_time)/DT_DMON):int((_INVISIBLE_SECONDS_TO_RED+_visible_time+1)/DT_DMON)] = [True] * int(1/DT_DMON)
+    op_vector[int((_INVISIBLE_SECONDS_TO_RED+_visible_time+1)/DT_DMON):int((_INVISIBLE_SECONDS_TO_RED+_visible_time+0.5)/DT_DMON)] = [False] * int(0.5/DT_DMON)
+    events_output = run_DState_seq(ds_vector, interaction_vector, op_vector, always_false)
+    self.assertTrue(len(events_output[int(_INVISIBLE_SECONDS_TO_ORANGE*0.5/DT_DMON)])==0)
+    self.assertEqual(events_output[int((_INVISIBLE_SECONDS_TO_ORANGE-0.1)/DT_DMON)][0].name, 'promptDriverUnresponsive')
+    self.assertEqual(events_output[int((_INVISIBLE_SECONDS_TO_RED-0.1)/DT_DMON)][0].name, 'driverUnresponsive')
+    self.assertEqual(events_output[int((_INVISIBLE_SECONDS_TO_RED+0.5*_visible_time)/DT_DMON)][0].name, 'driverUnresponsive')
+    self.assertEqual(events_output[int((_INVISIBLE_SECONDS_TO_RED+_visible_time+0.5)/DT_DMON)][0].name, 'driverUnresponsive')
+    self.assertTrue(len(events_output[int((_INVISIBLE_SECONDS_TO_RED+_visible_time+1+0.1)/DT_DMON)])==0)
+
+  # 7. op not engaged, always distracted driver
+  #  - dm should stay quiet when not engaged
+  def test_pure_dashcam_user(self):
+    events_output = run_DState_seq(always_distracted, always_false, always_false, always_false)
+    self.assertTrue(np.sum([len(event) for event in events_output])==0)
+
+  # 8. op engaged, car stops at traffic light, down to orange, no action, then car starts moving
+  #  - should only reach green when stopped, but continues counting down on launch
+  def test_long_traffic_light_victim(self):
+    _redlight_time = 60 # seconds
+    standstill_vector = always_true[:]
+    standstill_vector[int(_redlight_time/DT_DMON):] = [False] * int((_TEST_TIMESPAN-_redlight_time)/DT_DMON)
+    events_output = run_DState_seq(always_distracted, always_false, always_true, standstill_vector)
+    self.assertEqual(events_output[int((_DISTRACTED_TIME-_DISTRACTED_PRE_TIME_TILL_TERMINAL+1)/DT_DMON)][0].name, 'preDriverDistracted')
+    self.assertEqual(events_output[int((_redlight_time-0.1)/DT_DMON)][0].name, 'preDriverDistracted')
+    self.assertEqual(events_output[int((_redlight_time+0.5)/DT_DMON)][0].name, 'promptDriverDistracted')
+
+if __name__ == "__main__":
+  print 'MAX_TERMINAL_ALERTS', MAX_TERMINAL_ALERTS
+  unittest.main()

selfdrive/locationd/calibrationd.py

@@ -6,7 +6,6 @@ import numpy as np
 import selfdrive.messaging as messaging
 from selfdrive.locationd.calibration_helpers import Calibration
 from selfdrive.swaglog import cloudlog
-from selfdrive.services import service_list
 from common.params import Params
 from common.transformations.model import model_height
 from common.transformations.camera import view_frame_from_device_frame, get_view_frame_from_road_frame, \
@@ -64,7 +63,7 @@ class Calibrator(object):
       self.just_calibrated = True
 
   def handle_cam_odom(self, log):
-    trans, rot = log.cameraOdometry.trans, log.cameraOdometry.rot
+    trans, rot = log.trans, log.rot
     if np.linalg.norm(trans) > MIN_SPEED_FILTER and abs(rot[2]) < MAX_YAW_RATE_FILTER:
       new_vp = eon_intrinsics.dot(view_frame_from_device_frame.dot(trans))
       new_vp = new_vp[:2]/new_vp[2]
@@ -81,7 +80,7 @@ class Calibrator(object):
     else:
       return None
 
-  def send_data(self, livecalibration):
+  def send_data(self, pm):
     calib = get_calib_from_vp(self.vp)
     extrinsic_matrix = get_view_frame_from_road_frame(0, calib[1], calib[2], model_height)
 
@@ -92,27 +91,31 @@ class Calibrator(object):
     cal_send.liveCalibration.extrinsicMatrix = [float(x) for x in extrinsic_matrix.flatten()]
     cal_send.liveCalibration.rpyCalib = [float(x) for x in calib]
 
-    livecalibration.send(cal_send.to_bytes())
+    pm.send('liveCalibration', cal_send)
 
 
-def calibrationd_thread(gctx=None, addr="127.0.0.1"):
-  cameraodometry = messaging.sub_sock(service_list['cameraOdometry'].port, addr=addr, conflate=True)
-  livecalibration = messaging.pub_sock(service_list['liveCalibration'].port)
+def calibrationd_thread(sm=None, pm=None):
+  if sm is None:
+    sm = messaging.SubMaster(['cameraOdometry'])
+
+  if pm is None:
+    pm = messaging.PubMaster(['liveCalibration'])
+
   calibrator = Calibrator(param_put=True)
 
   # buffer with all the messages that still need to be input into the kalman
   while 1:
-    co = messaging.recv_one(cameraodometry)
+    sm.update()
 
-    new_vp = calibrator.handle_cam_odom(co)
+    new_vp = calibrator.handle_cam_odom(sm['cameraOdometry'])
     if DEBUG and new_vp is not None:
       print 'got new vp', new_vp
 
-    calibrator.send_data(livecalibration)
+    calibrator.send_data(pm)
 
 
-def main(gctx=None, addr="127.0.0.1"):
-  calibrationd_thread(gctx, addr)
+def main(sm=None, pm=None):
+  calibrationd_thread(sm, pm)
 
 
 if __name__ == "__main__":

selfdrive/locationd/locationd_yawrate.cc

@@ -10,7 +10,7 @@
 #include "locationd_yawrate.h"
 
 
-void Localizer::update_state(const Eigen::Matrix<double, 1, 2> &C, const double R, double current_time, double meas) {
+void Localizer::update_state(const Eigen::Matrix<double, 1, 4> &C, const double R, double current_time, double meas) {
   double dt = current_time - prev_update_time;
 
   if (dt < 0) {
@@ -19,36 +19,34 @@ void Localizer::update_state(const Eigen::Matrix<double, 1, 2> &C, const double
     prev_update_time = current_time;
   }
 
-  // x = A * x;
-  // P = A * P * A.transpose() + dt * Q;
-  // Simplify because A is unity
-  P = P + dt * Q;
+  x = A * x;
+  P = A * P * A.transpose() + dt * Q;
 
   double y = meas - C * x;
   double S = R + C * P * C.transpose();
-  Eigen::Vector2d K = P * C.transpose() * (1.0 / S);
+  Eigen::Vector4d K = P * C.transpose() * (1.0 / S);
   x = x + K * y;
   P = (I - K * C) * P;
 }
 
 void Localizer::handle_sensor_events(capnp::List<cereal::SensorEventData>::Reader sensor_events, double current_time) {
   for (cereal::SensorEventData::Reader sensor_event : sensor_events){
-    if (sensor_event.getType() == 4) {
+    if (sensor_event.getSensor() == 5) {
       sensor_data_time = current_time;
-
-      double meas = -sensor_event.getGyro().getV()[0];
+      double meas = -sensor_event.getGyroUncalibrated().getV()[0];
       update_state(C_gyro, R_gyro, current_time, meas);
     }
   }
 }
 
 void Localizer::handle_camera_odometry(cereal::CameraOdometry::Reader camera_odometry, double current_time) {
-  double R = 250.0 * pow(camera_odometry.getRotStd()[2], 2);
+  double R = 100.0 * pow(camera_odometry.getRotStd()[2], 2);
   double meas = camera_odometry.getRot()[2];
   update_state(C_posenet, R, current_time, meas);
 
   auto trans = camera_odometry.getTrans();
   posenet_speed = sqrt(trans[0]*trans[0] + trans[1]*trans[1] + trans[2]*trans[2]);
+  camera_odometry_time = current_time;
 }
 
 void Localizer::handle_controls_state(cereal::ControlsState::Reader controls_state, double current_time) {
@@ -59,17 +57,34 @@ void Localizer::handle_controls_state(cereal::ControlsState::Reader controls_sta
 
 
 Localizer::Localizer() {
-  A << 1, 0, 0, 1;
-  I << 1, 0, 0, 1;
-
-  Q << pow(0.1, 2.0), 0, 0, pow(0.005 / 100.0, 2.0);
-  P << pow(1.0, 2.0), 0, 0, pow(0.05, 2.0);
-
-  C_posenet << 1, 0;
-  C_gyro << 1, 1;
-  x << 0, 0;
-
-  R_gyro = pow(0.05, 2.0);
+  // States: [yaw rate, yaw rate diff, gyro bias, gyro bias diff]
+  A <<
+    1, 1, 0, 0,
+    0, 1, 0, 0,
+    0, 0, 1, 1,
+    0, 0, 0, 1;
+  I <<
+    1, 0, 0, 0,
+    0, 1, 0, 0,
+    0, 0, 1, 0,
+    0, 0, 0, 1;
+
+  Q <<
+    0, 0, 0, 0,
+    0, pow(0.1, 2.0), 0, 0,
+    0, 0, 0, 0,
+    0, 0, pow(0.0005 / 100.0, 2.0), 0;
+  P <<
+    pow(100.0, 2.0), 0, 0, 0,
+    0, pow(100.0, 2.0), 0, 0,
+    0, 0, pow(100.0, 2.0), 0,
+    0, 0, 0, pow(100.0, 2.0);
+
+  C_posenet << 1, 0, 0, 0;
+  C_gyro << 1, 0, 1, 0;
+  x << 0, 0, 0, 0;
+
+  R_gyro = pow(0.25, 2.0);
 }
 
 void Localizer::handle_log(cereal::Event::Reader event) {
@@ -118,20 +133,31 @@ extern "C" {
   }
   double localizer_get_bias(void * localizer) {
     Localizer * loc = (Localizer*) localizer;
-    return loc->x[1];
+    return loc->x[2];
   }
 
-  void localizer_set_yaw(void * localizer, double yaw) {
+  double * localizer_get_state(void * localizer) {
     Localizer * loc = (Localizer*) localizer;
-    loc->x[0] = yaw;
+    return loc->x.data();
   }
-  void localizer_set_bias(void * localizer, double bias) {
+
+  void localizer_set_state(void * localizer, double * state) {
     Localizer * loc = (Localizer*) localizer;
-    loc->x[1] = bias;
+    memcpy(loc->x.data(), state, 4 * sizeof(double));
   }
 
   double localizer_get_t(void * localizer) {
     Localizer * loc = (Localizer*) localizer;
     return loc->prev_update_time;
   }
+
+  double * localizer_get_P(void * localizer) {
+    Localizer * loc = (Localizer*) localizer;
+    return loc->P.data();
+  }
+
+  void localizer_set_P(void * localizer, double * P) {
+    Localizer * loc = (Localizer*) localizer;
+    memcpy(loc->P.data(), P, 16 * sizeof(double));
+  }
 }

selfdrive/locationd/locationd_yawrate.h

@@ -7,28 +7,29 @@
 
 class Localizer
 {
-  Eigen::Matrix2d A;
-  Eigen::Matrix2d I;
-  Eigen::Matrix2d Q;
-  Eigen::Matrix2d P;
-  Eigen::Matrix<double, 1, 2> C_posenet;
-  Eigen::Matrix<double, 1, 2> C_gyro;
+  Eigen::Matrix4d A;
+  Eigen::Matrix4d I;
+  Eigen::Matrix4d Q;
+  Eigen::Matrix<double, 1, 4> C_posenet;
+  Eigen::Matrix<double, 1, 4> C_gyro;
 
   double R_gyro;
 
-  void update_state(const Eigen::Matrix<double, 1, 2> &C, const double R, double current_time, double meas);
+  void update_state(const Eigen::Matrix<double, 1, 4> &C, const double R, double current_time, double meas);
   void handle_sensor_events(capnp::List<cereal::SensorEventData>::Reader sensor_events, double current_time);
   void handle_camera_odometry(cereal::CameraOdometry::Reader camera_odometry, double current_time);
   void handle_controls_state(cereal::ControlsState::Reader controls_state, double current_time);
 
 public:
-  Eigen::Vector2d x;
+  Eigen::Vector4d x;
+  Eigen::Matrix4d P;
   double steering_angle = 0;
   double car_speed = 0;
   double posenet_speed = 0;
   double prev_update_time = -1;
   double controls_state_time = -1;
   double sensor_data_time = -1;
+  double camera_odometry_time = -1;
 
   Localizer();
   void handle_log(cereal::Event::Reader event);

selfdrive/locationd/params_learner.cc

@@ -45,7 +45,7 @@ ParamsLearner::ParamsLearner(cereal::CarParams::Reader car_params,
 }
 
 bool ParamsLearner::update(double psi, double u, double sa) {
-  if (u > 10.0 && fabs(sa) < (DEGREES_TO_RADIANS * 15.)) {
+  if (u > 10.0 && fabs(sa) < (DEGREES_TO_RADIANS * 90.)) {
     double ao_diff = 2.0*cF0*cR0*l*u*x*(1.0*cF0*cR0*l*u*x*(ao - sa) + psi*sR*(cF0*cR0*pow(l, 2)*x - m*pow(u, 2)*(aF*cF0 - aR*cR0)))/(pow(sR, 2)*pow(cF0*cR0*pow(l, 2)*x - m*pow(u, 2)*(aF*cF0 - aR*cR0), 2));
     double new_ao = ao - alpha1 * ao_diff;
 

selfdrive/locationd/paramsd.cc

@@ -137,33 +137,32 @@ int main(int argc, char *argv[]) {
 
           // TODO: Fix in replay
           double sensor_data_age = localizer.controls_state_time - localizer.sensor_data_time;
+          double camera_odometry_age = localizer.controls_state_time - localizer.camera_odometry_time;
 
           double angle_offset_degrees = RADIANS_TO_DEGREES * learner.ao;
           double angle_offset_average_degrees = RADIANS_TO_DEGREES * learner.slow_ao;
 
-          // Send parameters at 10 Hz
-          if (save_counter % 10 == 0){
           capnp::MallocMessageBuilder msg;
           cereal::Event::Builder event = msg.initRoot<cereal::Event>();
           event.setLogMonoTime(nanos_since_boot());
           auto live_params = event.initLiveParameters();
           live_params.setValid(valid);
           live_params.setYawRate(localizer.x[0]);
-            live_params.setGyroBias(localizer.x[1]);
+          live_params.setGyroBias(localizer.x[2]);
           live_params.setSensorValid(sensor_data_age < 5.0);
           live_params.setAngleOffset(angle_offset_degrees);
           live_params.setAngleOffsetAverage(angle_offset_average_degrees);
           live_params.setStiffnessFactor(learner.x);
           live_params.setSteerRatio(learner.sR);
           live_params.setPosenetSpeed(localizer.posenet_speed);
-            live_params.setPosenetValid(posenet_invalid_count < 4);
+          live_params.setPosenetValid((posenet_invalid_count < 4) && (camera_odometry_age < 5.0));
 
           auto words = capnp::messageToFlatArray(msg);
           auto bytes = words.asBytes();
           zmq_send(live_parameters_sock_raw, bytes.begin(), bytes.size(), ZMQ_DONTWAIT);
-          }
 
           // Save parameters every minute
+          // TODO: Save in seperate thread
           if (save_counter % 6000 == 0) {
             json11::Json json = json11::Json::object {
               {"carVin", vin},

selfdrive/loggerd/uploader.py

@@ -93,9 +93,9 @@ def is_on_hotspot():
     return False
 
 class Uploader(object):
-  def __init__(self, dongle_id, private_key, root):
+  def __init__(self, dongle_id, root):
     self.dongle_id = dongle_id
-    self.api = Api(dongle_id, private_key)
+    self.api = Api(dongle_id)
     self.root = root
 
     self.upload_thread = None
@@ -146,14 +146,11 @@ class Uploader(object):
         return (key, fn, 0)
 
     if with_raw:
-      # then upload log files
+      # then upload the full log files, rear and front camera files
       for name, key, fn in self.gen_upload_files():
         if name  == "rlog.bz2":
           return (key, fn, 1)
-
-      # then upload rear and front camera files
-      for name, key, fn in self.gen_upload_files():
-        if name == "fcamera.hevc":
+        elif name == "fcamera.hevc":
           return (key, fn, 2)
         elif name == "dcamera.hevc":
           return (key, fn, 3)
@@ -241,13 +238,12 @@ def uploader_fn(exit_event):
 
   params = Params()
   dongle_id = params.get("DongleId")
-  private_key = open("/persist/comma/id_rsa").read()
 
-  if dongle_id is None or private_key is None:
-    cloudlog.info("uploader missing dongle_id or private_key")
-    raise Exception("uploader can't start without dongle id and private key")
+  if dongle_id is None:
+    cloudlog.info("uploader missing dongle_id")
+    raise Exception("uploader can't start without dongle id")
 
-  uploader = Uploader(dongle_id, private_key, ROOT)
+  uploader = Uploader(dongle_id, ROOT)
 
   backoff = 0.1
   while True:

selfdrive/manager.py

@@ -47,7 +47,7 @@ if __name__ == "__main__":
   if is_neos:
     version = int(open("/VERSION").read()) if os.path.isfile("/VERSION") else 0
     revision = int(open("/REVISION").read()) if version >= 10 else 0 # Revision only present in NEOS 10 and up
-    neos_update_required = version < 10 or (version == 10 and revision != 4)
+    neos_update_required = version < 10 or (version == 10 and revision < 4)
 
   if neos_update_required:
     # update continue.sh before updating NEOS
@@ -365,6 +365,7 @@ def manager_thread():
 
   # start frame
   pm_apply_packages('enable')
+  system("LD_LIBRARY_PATH= appops set ai.comma.plus.offroad SU allow")
   system("am start -n ai.comma.plus.frame/.MainActivity")
 
   if os.getenv("NOBOARD") is None:
@@ -498,6 +499,11 @@ def manager_update():
   update_ssh()
   update_apks()
 
+  uninstall = [app for app in get_installed_apks().keys() if app in ("com.spotify.music", "com.waze")]
+  for app in uninstall:
+    cloudlog.info("uninstalling %s" % app)
+    os.system("pm uninstall % s" % app)
+
 def manager_prepare():
   # build cereal first
   subprocess.check_call(["make", "-j4"], cwd=os.path.join(BASEDIR, "cereal"))
@@ -555,16 +561,12 @@ def main():
     params.put("IsMetric", "0")
   if params.get("RecordFront") is None:
     params.put("RecordFront", "0")
-  if params.get("IsFcwEnabled") is None:
-    params.put("IsFcwEnabled", "1")
   if params.get("HasAcceptedTerms") is None:
     params.put("HasAcceptedTerms", "0")
   if params.get("IsUploadRawEnabled") is None:
     params.put("IsUploadRawEnabled", "1")
   if params.get("IsUploadVideoOverCellularEnabled") is None:
     params.put("IsUploadVideoOverCellularEnabled", "1")
-  if params.get("IsDriverMonitoringEnabled") is None:
-    params.put("IsDriverMonitoringEnabled", "1")
   if params.get("IsGeofenceEnabled") is None:
     params.put("IsGeofenceEnabled", "-1")
   if params.get("SpeedLimitOffset") is None:

selfdrive/messaging.py

@@ -84,8 +84,8 @@ def recv_one_or_none(sock):
     return None
 
 
-class SubMaster():
-  def __init__(self, services, addr="127.0.0.1"):
+class SubMaster(object):
+  def __init__(self, services, ignore_alive=None, addr="127.0.0.1"):
     self.poller = zmq.Poller()
     self.frame = -1
     self.updated = {s : False for s in services}
@@ -97,6 +97,12 @@ class SubMaster():
     self.data = {}
     self.logMonoTime = {}
     self.valid = {}
+
+    if ignore_alive is not None:
+      self.ignore_alive = ignore_alive
+    else:
+      self.ignore_alive = []
+
     for s in services:
       # TODO: get address automatically from service_list
       if addr is not None:
@@ -141,7 +147,7 @@ class SubMaster():
   def all_alive(self, service_list=None):
     if service_list is None:  # check all
       service_list = self.alive.keys()
-    return all(self.alive[s] for s in service_list)
+    return all(self.alive[s] for s in service_list if s not in self.ignore_alive)
 
   def all_valid(self, service_list=None):
     if service_list is None:  # check all
@@ -152,3 +158,16 @@ class SubMaster():
     if service_list is None:  # check all
       service_list = self.alive.keys()
     return self.all_alive(service_list=service_list) and self.all_valid(service_list=service_list)
+
+
+class PubMaster():
+  def __init__(self, services):
+    self.sock = {}
+    for s in services:
+      self.sock[s] = pub_sock(service_list[s].port)
+
+  def send(self, s, dat):
+    # accept either bytes or capnp builder
+    if not isinstance(dat, str):
+      dat = dat.to_bytes()
+    self.sock[s].send(dat)

selfdrive/service_list.yaml

@@ -16,7 +16,7 @@ thermal: [8005, true, 2., 1]
 can: [8006, true, 100.]
 controlsState: [8007, true, 100., 100]
 #liveEvent: [8008, true, 0.]
-model: [8009, true, 20.]
+model: [8009, true, 20., 5]
 features: [8010, true, 0.]
 health: [8011, true, 2., 1]
 radarState: [8012, true, 20.]

selfdrive/test/test_car_models.py

@@ -14,6 +14,7 @@ from selfdrive.services import service_list
 import selfdrive.messaging as messaging
 from common.params import Params
 from common.basedir import BASEDIR
+from common.fingerprints import all_known_cars
 from selfdrive.car.honda.values import CAR as HONDA
 from selfdrive.car.toyota.values import CAR as TOYOTA
 from selfdrive.car.gm.values import CAR as GM
@@ -54,6 +55,7 @@ def get_route_logs(route_name):
         with open(log_path, "w") as f:
           f.write(r.content)
       else:
+        print "failed to download test log %s" % route_name
         sys.exit(-1)
 
 routes = {
@@ -140,6 +142,10 @@ routes = {
     'carFingerprint': HONDA.CRV_HYBRID,
     'enableCamera': True,
   },
+  "99e3eaed7396619e|2019-08-13--15-07-03": {
+    'carFingerprint': HONDA.FIT,
+    'enableCamera': True,
+  },
   "2ac95059f70d76eb|2018-02-05--15-03-29": {
     'carFingerprint': HONDA.ACURA_ILX,
     'enableCamera': True,
@@ -351,6 +357,21 @@ routes = {
     'enableCamera': False,
     'enableDsu': False,
   },
+  "2e07163a1ba9a780|2019-08-25--13-15-13": {
+    'carFingerprint': TOYOTA.LEXUS_IS,
+    'enableCamera': True,
+    'enableDsu': False,
+  },
+  "2e07163a1ba9a780|2019-08-29--09-35-42": {
+    'carFingerprint': TOYOTA.LEXUS_IS,
+    'enableCamera': False,
+    'enableDsu': False,
+  },
+  "1dd19ceed0ee2b48|2018-12-22--17-36-49": {
+    'carFingerprint': TOYOTA.LEXUS_IS, # 300 hybrid
+    'enableCamera': True,
+    'enableDsu': False,
+  },
   "791340bc01ed993d|2019-03-10--16-28-08": {
     'carFingerprint': SUBARU.IMPREZA,
     'enableCamera': True,
@@ -370,32 +391,58 @@ passive_routes = [
   #"bfa17080b080f3ec|2018-06-28--23-27-47",
 ]
 
-public_routes = [
-  "f1b4c567731f4a1b|2018-06-06--14-43-46",
-  "f1b4c567731f4a1b|2018-04-18--11-29-37",
-  "f1b4c567731f4a1b|2018-04-18--11-29-37",
-  "7ed9cdf8d0c5f43e|2018-05-17--09-31-36",
-  "38bfd238edecbcd7|2018-08-22--09-45-44",
-  "38bfd238edecbcd7|2018-08-29--22-02-15",
-  "b0f5a01cf604185c|2018-01-26--00-54-32",
-  "b0f5a01cf604185c|2018-01-26--10-54-38",
-  "b0f5a01cf604185c|2018-01-26--10-59-31",
-  "56fb1c86a9a86404|2017-11-10--10-18-43",
-  "b0f5a01cf604185c|2017-12-18--20-32-32",
-  "b0c9d2329ad1606b|2019-04-02--13-24-43",
-  "791340bc01ed993d|2019-03-10--16-28-08",
+# TODO: replace all these with public routes
+# TODO: add routes for untested cars: HONDA ACCORD 2018 HYBRID TOURING and CHRYSLER PACIFICA 2018
+non_public_routes = [
+  "0607d2516fc2148f|2019-02-13--23-03-16", # CHRYSLER PACIFICA HYBRID 2019
+  "3e9592a1c78a3d63|2018-02-08--20-28-24", # HONDA PILOT 2017 TOURING
+  "aa20e335f61ba898|2019-02-05--16-59-04", # BUICK REGAL ESSENCE 2018
+  "1851183c395ef471|2018-05-31--18-07-21", # HONDA CR-V 2017 EX
+  "9d5fb4f0baa1b3e1|2019-01-14--17-45-59", # KIA SORENTO GT LINE 2018
+  "b4c18bf13d5955da|2018-07-29--13-39-46", # TOYOTA C-HR HYBRID 2018
+  "5a2cfe4bb362af9e|2018-02-02--23-41-07", # ACURA RDX 2018 ACURAWATCH PLUS
+  "362d23d4d5bea2fa|2018-08-10--13-31-40", # TOYOTA HIGHLANDER HYBRID 2018
+  "aa20e335f61ba898|2018-12-17--21-10-37", # BUICK REGAL ESSENCE 2018
+  "215cd70e9c349266|2018-11-25--22-22-12", # KIA STINGER GT2 2018
+  "192a598e34926b1e|2019-04-04--13-27-39", # JEEP GRAND CHEROKEE 2019
+  "34a84d2b765df688|2018-08-28--12-41-00", # HONDA PILOT 2019 ELITE
+  "b0c9d2329ad1606b|2019-01-06--10-11-23", # CHRYSLER PACIFICA HYBRID 2017
+  "31390e3eb6f7c29a|2019-01-23--08-56-00", # KIA OPTIMA SX 2019
+  "fd10b9a107bb2e49|2018-07-24--16-32-42", # TOYOTA C-HR 2018
+  "9f7a7e50a51fb9db|2019-01-17--18-34-21", # JEEP GRAND CHEROKEE V6 2018
+  "aadda448b49c99ad|2018-10-25--17-16-22", # CHEVROLET MALIBU PREMIER 2017
+  "362d23d4d5bea2fa|2018-09-02--17-03-55", # TOYOTA HIGHLANDER HYBRID 2018
+  "1582e1dc57175194|2018-08-15--07-46-07", # HONDA ACCORD 2018 LX 1.5T
+  "fd10b9a107bb2e49|2018-07-24--20-32-08", # TOYOTA C-HR 2018
+  "265007255e794bce|2018-11-24--22-08-31", # CADILLAC ATS Premium Performance 2018
+  "53ac3251e03f95d7|2019-01-10--13-43-32", # HYUNDAI ELANTRA LIMITED ULTIMATE 2017
+  "21aa231dee2a68bd|2018-01-30--04-54-41", # HONDA ODYSSEY 2018 EX-L
+  "900ad17e536c3dc7|2018-04-12--22-02-36", # HONDA RIDGELINE 2017 BLACK EDITION
+  "975b26878285314d|2018-12-25--14-42-13", # CHRYSLER PACIFICA HYBRID 2018
+  "8ae193ceb56a0efe|2018-06-18--20-07-32", # TOYOTA RAV4 HYBRID 2017
+  "a893a80e5c5f72c8|2019-01-14--20-02-59", # HYUNDAI GENESIS 2018
+  "49c73650e65ff465|2018-11-19--16-58-04", # HOLDEN ASTRA RS-V BK 2017
+  "d2d8152227f7cb82|2018-07-25--13-40-56", # TOYOTA CAMRY 2018
+  "07cb8a788c31f645|2018-06-17--18-50-29", # mock
+  "c9d60e5e02c04c5c|2018-01-08--16-01-49", # HONDA CR-V 2016 TOURING
+  "1632088eda5e6c4d|2018-06-07--08-03-18", # HONDA CIVIC HATCHBACK 2017 SEDAN/COUPE 2019
+  "fbd011384db5e669|2018-07-26--20-51-48", # TOYOTA CAMRY HYBRID 2018
 ]
 
 if __name__ == "__main__":
 
+  # TODO: add routes for untested cars and fail test if we have an untested car
+  tested_cars = [keys["carFingerprint"] for route, keys in routes.items()]
+  for car_model in all_known_cars():
+    if car_model not in tested_cars:
+      print "***** WARNING: %s not tested *****" % car_model
+
   results = {}
   for route, checks in routes.items():
-
-    if route not in public_routes:
-      print "route not public", route
-      continue
-
+    if route not in non_public_routes:
       get_route_logs(route)
+    elif "UNLOGGER_PATH" not in os.environ:
+      continue
 
     for _ in range(3):
       shutil.rmtree('/data/params')
@@ -420,6 +467,9 @@ if __name__ == "__main__":
 
       # Start unlogger
       print "Start unlogger"
+      if route in non_public_routes:
+        unlogger_cmd = [os.path.join(BASEDIR, os.environ['UNLOGGER_PATH']), '%s' % route, '--disable', 'frame,plan,pathPlan,liveLongitudinalMpc,radarState,controlsState,liveTracks,liveMpc,sendcan,carState,carControl', '--no-interactive']
+      else:
         unlogger_cmd = [os.path.join(BASEDIR, 'tools/replay/unlogger.py'), '%s' % route, '/tmp', '--disable', 'frame,plan,pathPlan,liveLongitudinalMpc,radarState,controlsState,liveTracks,liveMpc,sendcan,carState,carControl', '--no-interactive']
       unlogger = subprocess.Popen(unlogger_cmd, preexec_fn=os.setsid)
 

selfdrive/test/tests/plant/test_longitudinal.py

@@ -332,7 +332,6 @@ class LongitudinalControl(unittest.TestCase):
     shutil.rmtree('/data/params', ignore_errors=True)
     params = Params()
     params.put("Passive", "1" if os.getenv("PASSIVE") else "0")
-    params.put("IsFcwEnabled", "1")
 
     manager.gctx = {}
     manager.prepare_managed_process('radard')

selfdrive/test/tests/process_replay/README.md

@@ -13,3 +13,12 @@ Currently the following processes are tested:
 * plannerd
 * calibrationd
 
+## Forks
+
+openpilot forks can use this test with their own reference logs
+
+To generate new logs:
+
+`./update-refs.py --no-upload`
+
+Then, check in the new logs using git-lfs. Make sure to also include the updated `ref_commit` file.

selfdrive/test/tests/process_replay/compare_logs.py

@@ -11,7 +11,6 @@ else:
 
 from tools.lib.logreader import LogReader
 
-
 def save_log(dest, log_msgs):
   dat = ""
   for msg in log_msgs:
@@ -21,25 +20,42 @@ def save_log(dest, log_msgs):
   with open(dest, "w") as f:
    f.write(dat)
 
+def remove_ignored_fields(msg, ignore):
+  msg = msg.as_builder() 
+  for key, val in ignore:
+    attr = msg
+    keys = key.split(".")
+    if msg.which() not in key and len(keys) > 1:
+      continue
+    
+    for k in keys[:-1]:
+      try:
+        attr = getattr(msg, k)
+      except:
+        break
+    else:
+      setattr(attr, keys[-1], val)
+  return msg.as_reader()
+
 def compare_logs(log1, log2, ignore=[]):
   assert len(log1) == len(log2), "logs are not same length"
 
+  ignore_fields = [k for k, v in ignore]
   diff = []
   for msg1, msg2 in tqdm(zip(log1, log2)):
     assert msg1.which() == msg2.which(), "msgs not aligned between logs"
 
-    msg1_bytes = msg1.as_builder().to_bytes()
-    msg2_bytes = msg2.as_builder().to_bytes()
+    msg1_bytes = remove_ignored_fields(msg1, ignore).as_builder().to_bytes()
+    msg2_bytes = remove_ignored_fields(msg2, ignore).as_builder().to_bytes()
 
     if msg1_bytes != msg2_bytes:
       msg1_dict = msg1.to_dict(verbose=True)
       msg2_dict = msg2.to_dict(verbose=True)
-      dd = dictdiffer.diff(msg1_dict, msg2_dict, ignore=ignore, tolerance=0)
+      dd = dictdiffer.diff(msg1_dict, msg2_dict, ignore=ignore_fields, tolerance=0)
       diff.extend(dd)
   return diff
 
 if __name__ == "__main__":
   log1 = list(LogReader(sys.argv[1]))
   log2 = list(LogReader(sys.argv[2]))
-
   compare_logs(log1, log2, sys.argv[3:])

selfdrive/test/tests/process_replay/process_replay.py

@@ -1,14 +1,15 @@
 #!/usr/bin/env python2
-import gc
 import os
-import time
+import threading
+import importlib
+import zmq
 
 if "CI" in os.environ:
   tqdm = lambda x: x
 else:
   from tqdm import tqdm
 
-from cereal import car
+from cereal import car, log
 from selfdrive.car.car_helpers import get_car
 import selfdrive.manager as manager
 import selfdrive.messaging as messaging
@@ -18,61 +19,154 @@ from collections import namedtuple
 
 ProcessConfig = namedtuple('ProcessConfig', ['proc_name', 'pub_sub', 'ignore', 'init_callback', 'should_recv_callback'])
 
-def fingerprint(msgs, pub_socks, sub_socks):
+class FakeSocket:
+  def __init__(self, wait=True):
+    self.data = []
+    self.wait = wait
+    self.recv_called = threading.Event()
+    self.recv_ready = threading.Event()
+
+  def recv(self, block=None):
+    if block == zmq.NOBLOCK:
+      raise zmq.error.Again
+
+    if self.wait:
+      self.recv_called.set()
+      self.recv_ready.wait()
+      self.recv_ready.clear()
+    return self.data.pop()
+
+  def send(self, data):
+    if self.wait:
+      self.recv_called.wait()
+      self.recv_called.clear()
+
+    self.data.append(data)
+
+    if self.wait:
+      self.recv_ready.set()
+
+  def wait_for_recv(self):
+    self.recv_called.wait()
+
+class DumbSocket:
+  def __init__(self, s=None):
+    if s is not None:
+      dat = messaging.new_message()
+      dat.init(s)
+      self.data = dat.to_bytes()
+
+  def recv(self, block=None):
+    return self.data
+
+  def send(self, dat):
+    pass
+
+class FakeSubMaster(messaging.SubMaster):
+  def __init__(self, services):
+    super(FakeSubMaster, self).__init__(services, addr=None)
+    self.sock = {s: DumbSocket(s) for s in services}
+    self.update_called = threading.Event()
+    self.update_ready = threading.Event()
+
+    self.wait_on_getitem = False
+
+  def __getitem__(self, s):
+    # hack to know when fingerprinting is done
+    if self.wait_on_getitem:
+      self.update_called.set()
+      self.update_ready.wait()
+      self.update_ready.clear()
+    return self.data[s]
+
+  def update(self, timeout=-1):
+    self.update_called.set()
+    self.update_ready.wait()
+    self.update_ready.clear()
+
+  def update_msgs(self, cur_time, msgs):
+    self.update_called.wait()
+    self.update_called.clear()
+    super(FakeSubMaster, self).update_msgs(cur_time, msgs)
+    self.update_ready.set()
+
+  def wait_for_update(self):
+    self.update_called.wait()
+
+class FakePubMaster(messaging.PubMaster):
+  def __init__(self, services):
+    self.data = {}
+    self.sock = {}
+    self.last_updated = None
+    for s in services:
+      data = messaging.new_message()
+      try:
+        data.init(s)
+      except:
+        data.init(s, 0)
+      self.data[s] = data.as_reader()
+      self.sock[s] = DumbSocket()
+    self.send_called = threading.Event()
+    self.get_called = threading.Event()
+
+  def send(self, s, dat):
+    self.last_updated = s
+    if isinstance(dat, str):
+      self.data[s] = log.Event.from_bytes(dat)
+    else:
+      self.data[s] = dat.as_reader()
+    self.send_called.set()
+    self.get_called.wait()
+    self.get_called.clear()
+
+  def wait_for_msg(self):
+    self.send_called.wait()
+    self.send_called.clear()
+    dat = self.data[self.last_updated]
+    self.get_called.set()
+    return dat
+
+def fingerprint(msgs, fsm, can_sock):
   print "start fingerprinting"
-  manager.prepare_managed_process("logmessaged")
-  manager.start_managed_process("logmessaged")
+  fsm.wait_on_getitem = True
 
-  can = pub_socks["can"]
-  logMessage = messaging.sub_sock(service_list["logMessage"].port)
+  # populate fake socket with data for fingerprinting
+  canmsgs = filter(lambda msg: msg.which() == "can", msgs)
+  can_sock.recv_called.wait()
+  can_sock.recv_called.clear()
+  can_sock.data = [msg.as_builder().to_bytes() for msg in canmsgs[:300]]
+  can_sock.recv_ready.set()
+  can_sock.wait = False
 
-  time.sleep(1)
-  messaging.drain_sock(logMessage)
+  # we know fingerprinting is done when controlsd sets sm['pathPlan'].sensorValid
+  fsm.update_called.wait()
+  fsm.update_called.clear()
 
-  # controlsd waits for a health packet before fingerprinting
-  msg = messaging.new_message()
-  msg.init("health")
-  pub_socks["health"].send(msg.to_bytes())
+  fsm.wait_on_getitem = False
+  can_sock.wait = True
+  can_sock.data = []
 
-  canmsgs = filter(lambda msg: msg.which() == "can", msgs)
-  for msg in canmsgs[:200]:
-    can.send(msg.as_builder().to_bytes())
-
-    time.sleep(0.005)
-    log = messaging.recv_one_or_none(logMessage)
-    if log is not None and "fingerprinted" in log.logMessage:
-      break
-  manager.kill_managed_process("logmessaged")
+  fsm.update_ready.set()
   print "finished fingerprinting"
 
-def get_car_params(msgs, pub_socks, sub_socks):
-  sendcan = pub_socks.get("sendcan", None)
-  if sendcan is None:
-    sendcan = messaging.pub_sock(service_list["sendcan"].port)
-  logcan = sub_socks.get("can", None)
-  if logcan is None:
-    logcan = messaging.sub_sock(service_list["can"].port)
-  can = pub_socks.get("can", None)
-  if can is None:
-    can = messaging.pub_sock(service_list["can"].port)
-
-  time.sleep(0.5)
+def get_car_params(msgs, fsm, can_sock):
+  can = FakeSocket(wait=False)
+  sendcan = FakeSocket(wait=False)
 
-  canmsgs = filter(lambda msg: msg.which() == "can", msgs)
-  for m in canmsgs[:200]:
+  canmsgs = filter(lambda msg: msg.which() == 'can', msgs)
+  for m in canmsgs[:300]:
     can.send(m.as_builder().to_bytes())
-  _, CP = get_car(logcan, sendcan)
+  _, CP = get_car(can, sendcan)
   Params().put("CarParams", CP.to_bytes())
-  time.sleep(0.5)
-  messaging.drain_sock(logcan)
 
 def radar_rcv_callback(msg, CP):
   if msg.which() != "can":
     return []
+  elif CP.radarOffCan:
+    return ["radarState", "liveTracks"]
 
-  # hyundai and subaru don't have radar
-  radar_msgs = {"honda": [0x445], "toyota": [0x19f, 0x22f], "gm": [0x475],
-                "hyundai": [], "chrysler": [0x2d4], "subaru": []}.get(CP.carName, None)
+  radar_msgs = {"honda": [0x445], "toyota": [0x19f, 0x22f], "gm": [0x474],
+                "chrysler": [0x2d4]}.get(CP.carName, None)
 
   if radar_msgs is None:
     raise NotImplementedError
@@ -80,24 +174,16 @@ def radar_rcv_callback(msg, CP):
   for m in msg.can:
     if m.src == 1 and m.address in radar_msgs:
       return ["radarState", "liveTracks"]
-
   return []
 
-def plannerd_rcv_callback(msg, CP):
-  if msg.which() in ["model", "radarState"]:
-    time.sleep(0.005)
-  else:
-    time.sleep(0.002)
-  return {"model": ["pathPlan"], "radarState": ["plan"]}.get(msg.which(), [])
-
 CONFIGS = [
   ProcessConfig(
     proc_name="controlsd",
     pub_sub={
-      "can": ["controlsState", "carState", "carControl", "sendcan"],
-      "thermal":  [], "health": [], "liveCalibration": [], "driverMonitoring": [], "plan": [], "pathPlan": []
+      "can": ["controlsState", "carState", "carControl", "sendcan", "carEvents", "carParams"],
+      "thermal":  [], "health": [], "liveCalibration": [], "driverMonitoring": [], "plan": [], "pathPlan": [], "gpsLocation": [],
     },
-    ignore=["logMonoTime", "controlsState.startMonoTime", "controlsState.cumLagMs"],
+    ignore=[("logMonoTime", 0), ("valid", True), ("controlsState.startMonoTime", 0), ("controlsState.cumLagMs", 0)],
     init_callback=fingerprint,
     should_recv_callback=None,
   ),
@@ -107,7 +193,7 @@ CONFIGS = [
       "can": ["radarState", "liveTracks"],
       "liveParameters":  [], "controlsState":  [], "model":  [],
     },
-    ignore=["logMonoTime", "radarState.cumLagMs"],
+    ignore=[("logMonoTime", 0), ("valid", True), ("radarState.cumLagMs", 0)],
     init_callback=get_car_params,
     should_recv_callback=radar_rcv_callback,
   ),
@@ -117,69 +203,82 @@ CONFIGS = [
       "model": ["pathPlan"], "radarState": ["plan"],
       "carState": [], "controlsState": [], "liveParameters": [],
     },
-    ignore=["logMonoTime", "valid", "plan.processingDelay"],
+    ignore=[("logMonoTime", 0), ("valid", True), ("plan.processingDelay", 0)],
     init_callback=get_car_params,
-    should_recv_callback=plannerd_rcv_callback,
+    should_recv_callback=None,
   ),
   ProcessConfig(
     proc_name="calibrationd",
     pub_sub={
       "cameraOdometry": ["liveCalibration"]
     },
-    ignore=["logMonoTime"],
+    ignore=[("logMonoTime", 0), ("valid", True)],
     init_callback=get_car_params,
     should_recv_callback=None,
   ),
 ]
 
 def replay_process(cfg, lr):
-  gc.disable()  # gc can occasionally cause canparser to timeout
+  sub_sockets = [s for _, sub in cfg.pub_sub.items() for s in sub]
+  pub_sockets = [s for s in cfg.pub_sub.keys() if s != 'can']
 
-  pub_socks, sub_socks = {}, {}
-  for pub, sub in cfg.pub_sub.iteritems():
-    pub_socks[pub] = messaging.pub_sock(service_list[pub].port)
-
-    for s in sub:
-      sub_socks[s] = messaging.sub_sock(service_list[s].port)
+  fsm = FakeSubMaster(pub_sockets)
+  fpm = FakePubMaster(sub_sockets)
+  args = (fsm, fpm)
+  if 'can' in cfg.pub_sub.keys():
+    can_sock = FakeSocket()
+    args = (fsm, fpm, can_sock)
 
   all_msgs = sorted(lr, key=lambda msg: msg.logMonoTime)
-  pub_msgs = filter(lambda msg: msg.which() in pub_socks.keys(), all_msgs)
+  pub_msgs = filter(lambda msg: msg.which() in cfg.pub_sub.keys(), all_msgs)
 
   params = Params()
   params.manager_start()
   params.put("Passive", "0")
 
-  manager.gctx = {}
+  os.environ['NO_RADAR_SLEEP'] = "1"
   manager.prepare_managed_process(cfg.proc_name)
-  manager.start_managed_process(cfg.proc_name)
-  time.sleep(3)   # Wait for started process to be ready
+  mod = importlib.import_module(manager.managed_processes[cfg.proc_name])
+  thread = threading.Thread(target=mod.main, args=args)
+  thread.daemon = True
+  thread.start()
 
   if cfg.init_callback is not None:
-    cfg.init_callback(all_msgs, pub_socks, sub_socks)
+    if 'can' not in cfg.pub_sub.keys():
+      can_sock = None
+    cfg.init_callback(all_msgs, fsm, can_sock)
 
   CP = car.CarParams.from_bytes(params.get("CarParams", block=True))
 
-  log_msgs = []
+  # wait for started process to be ready
+  if 'can' in cfg.pub_sub.keys():
+    can_sock.wait_for_recv()
+  else:
+    fsm.wait_for_update()
+
+  log_msgs, msg_queue = [], []
   for msg in tqdm(pub_msgs):
     if cfg.should_recv_callback is not None:
       recv_socks = cfg.should_recv_callback(msg, CP)
     else:
-      recv_socks = cfg.pub_sub[msg.which()]
+      recv_socks = [s for s in cfg.pub_sub[msg.which()] if
+                      (fsm.frame + 1) % int(service_list[msg.which()].frequency / service_list[s].frequency) == 0]
+
+    should_recv = bool(len(recv_socks))
 
-    pub_socks[msg.which()].send(msg.as_builder().to_bytes())
+    if msg.which() == 'can':
+      can_sock.send(msg.as_builder().to_bytes())
+    else:
+      msg_queue.append(msg.as_builder())
 
-    if len(recv_socks):
-      # TODO: add timeout
-      for sock in recv_socks:
-        m = messaging.recv_one(sub_socks[sock])
+    if should_recv:
+      fsm.update_msgs(0, msg_queue)
+      msg_queue = []
 
-        # make these values fixed for faster comparison
-        m_builder = m.as_builder()
-        m_builder.logMonoTime = 0
-        m_builder.valid = True
-        log_msgs.append(m_builder.as_reader())
+      recv_cnt = len(recv_socks)
+      while recv_cnt > 0:
+        m = fpm.wait_for_msg()
+        log_msgs.append(m)
 
-  gc.enable()
-  manager.kill_managed_process(cfg.proc_name)
+        recv_cnt -= m.which() in recv_socks
   return log_msgs
-

selfdrive/test/tests/process_replay/ref_commit

@@ -1 +1 @@
-e3388c62ffb80f4b3ca8721da56a581a93c44e79
+8a11bcbc9833e154e10b59a8babb2b4545372f56

selfdrive/ui/spinner/Makefile

@@ -0,0 +1,74 @@
+CC = clang
+CXX = clang++
+
+PHONELIBS = ../../../phonelibs
+
+WARN_FLAGS = -Werror=implicit-function-declaration \
+             -Werror=incompatible-pointer-types \
+             -Werror=int-conversion \
+             -Werror=return-type \
+             -Werror=format-extra-args
+
+CFLAGS = -std=gnu11 -fPIC -O2 $(WARN_FLAGS)
+CXXFLAGS = -std=c++11 -fPIC -O2 $(WARN_FLAGS)
+
+NANOVG_FLAGS = -I$(PHONELIBS)/nanovg
+
+OPENGL_LIBS = -lGLESv3
+
+FRAMEBUFFER_LIBS = -lutils -lgui -lEGL
+
+OBJS = spinner.o \
+       ../../common/framebuffer.o \
+       $(PHONELIBS)/nanovg/nanovg.o \
+       ../../common/spinner.o \
+       opensans_semibold.o \
+       img_spinner_track.o \
+       img_spinner_comma.o
+
+DEPS := $(OBJS:.o=.d)
+
+.PHONY: all
+all: spinner
+
+spinner: $(OBJS)
+	@echo "[ LINK ] $@"
+	$(CXX) -fPIC -o '$@' $^ \
+	      -s \
+        $(FRAMEBUFFER_LIBS) \
+        -L/system/vendor/lib64 \
+        $(OPENGL_LIBS) \
+        -lm -llog
+
+../../common/framebuffer.o: ../../common/framebuffer.cc
+	@echo "[ CXX ] $@"
+	$(CXX) $(CXXFLAGS) -MMD \
+           -I$(PHONELIBS)/android_frameworks_native/include \
+           -I$(PHONELIBS)/android_system_core/include \
+           -I$(PHONELIBS)/android_hardware_libhardware/include \
+           -c -o '$@' '$<'
+
+opensans_semibold.o: ../../assets/fonts/opensans_semibold.ttf
+	@echo "[ bin2o ] $@"
+	cd '$(dir $<)' && ld -r -b binary '$(notdir $<)' -o '$(abspath $@)'
+
+img_spinner_track.o: ../../assets/img_spinner_track.png
+	@echo "[ bin2o ] $@"
+	cd '$(dir $<)' && ld -r -b binary '$(notdir $<)' -o '$(abspath $@)'
+
+img_spinner_comma.o: ../../assets/img_spinner_comma.png
+	@echo "[ bin2o ] $@"
+	cd '$(dir $<)' && ld -r -b binary '$(notdir $<)' -o '$(abspath $@)'
+
+%.o: %.c
+	@echo "[ CC ] $@"
+	$(CC) $(CFLAGS) -MMD \
+          -I../.. \
+          $(NANOVG_FLAGS) \
+          -c -o '$@' '$<'
+
+.PHONY: clean
+clean:
+	rm -f spinner $(OBJS) $(DEPS)
+
+-include $(DEPS)

selfdrive/ui/spinner/spinner.c

@@ -9,92 +9,13 @@
 #include <EGL/egl.h>
 #include <EGL/eglext.h>
 
-#include "nanovg.h"
-#define NANOVG_GLES3_IMPLEMENTATION
-#include "nanovg_gl.h"
-#include "nanovg_gl_utils.h"
-
 #include "common/framebuffer.h"
+#include "common/spinner.h"
 
 int main(int argc, char** argv) {
   int err;
 
-  const char* spintext = NULL;
-  if (argc >= 2) {
-    spintext = argv[1];
-  }
-
-  // spinner
-  int fb_w, fb_h;
-  EGLDisplay display;
-  EGLSurface surface;
-  FramebufferState *fb = framebuffer_init("spinner", 0x00001000, false,
-                     &display, &surface, &fb_w, &fb_h);
-  assert(fb);
-
-  NVGcontext *vg = nvgCreateGLES3(NVG_ANTIALIAS | NVG_STENCIL_STROKES);
-  assert(vg);
-
-  int font = nvgCreateFont(vg, "Bold", "../../assets/fonts/opensans_semibold.ttf");
-  assert(font >= 0);
-
-  int spinner_img = nvgCreateImage(vg, "../../assets/img_spinner_track.png", 0);
-  assert(spinner_img >= 0);
-  int spinner_img_s = 360;
-  int spinner_img_x = ((fb_w/2)-(spinner_img_s/2));
-  int spinner_img_y = 260;
-  int spinner_img_xc = (fb_w/2);
-  int spinner_img_yc = (fb_h/2)-100;
-  int spinner_comma_img = nvgCreateImage(vg, "../../assets/img_spinner_comma.png", 0);
-  assert(spinner_comma_img >= 0);
-
-  for (int cnt = 0; ; cnt++) {
-    glClearColor(0.1, 0.1, 0.1, 1.0);
-    glClear(GL_STENCIL_BUFFER_BIT | GL_COLOR_BUFFER_BIT);
-    glEnable(GL_BLEND);
-    glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
-    nvgBeginFrame(vg, fb_w, fb_h, 1.0f);
-
-    // background
-    nvgBeginPath(vg);
-    NVGpaint bg = nvgLinearGradient(vg, fb_w, 0, fb_w, fb_h,
-    nvgRGBA(0, 0, 0, 175), nvgRGBA(0, 0, 0, 255));
-    nvgFillPaint(vg, bg);
-    nvgRect(vg, 0, 0, fb_w, fb_h);
-    nvgFill(vg);
-
-    // spin track
-    nvgSave(vg);
-    nvgTranslate(vg, spinner_img_xc, spinner_img_yc);
-    nvgRotate(vg, (3.75*M_PI * cnt/120.0));
-    nvgTranslate(vg, -spinner_img_xc, -spinner_img_yc);
-    NVGpaint spinner_imgPaint = nvgImagePattern(vg, spinner_img_x, spinner_img_y,
-      spinner_img_s, spinner_img_s, 0, spinner_img, 0.6f);
-    nvgBeginPath(vg);
-    nvgFillPaint(vg, spinner_imgPaint);
-    nvgRect(vg, spinner_img_x, spinner_img_y, spinner_img_s, spinner_img_s);
-    nvgFill(vg);
-    nvgRestore(vg);
-
-    // comma
-    NVGpaint comma_imgPaint = nvgImagePattern(vg, spinner_img_x, spinner_img_y,
-      spinner_img_s, spinner_img_s, 0, spinner_comma_img, 1.0f);
-    nvgBeginPath(vg);
-    nvgFillPaint(vg, comma_imgPaint);
-    nvgRect(vg, spinner_img_x, spinner_img_y, spinner_img_s, spinner_img_s);
-    nvgFill(vg);
-
-    // message
-    if (spintext) {
-      nvgTextAlign(vg, NVG_ALIGN_CENTER | NVG_ALIGN_TOP);
-      nvgFontSize(vg, 96.0f);
-      nvgText(vg, fb_w/2, (fb_h*2/3)+24, spintext, NULL);
-    }
-
-    nvgEndFrame(vg);
-    eglSwapBuffers(display, surface);
-    assert(glGetError() == GL_NO_ERROR);
-  }
+  spin(argc, argv);
 
   return 0;
 }

selfdrive/ui/ui.c

@@ -41,7 +41,6 @@
 #define STATUS_ENGAGED 2
 #define STATUS_WARNING 3
 #define STATUS_ALERT 4
-#define STATUS_MAX 5
 
 #define ALERTSIZE_NONE 0
 #define ALERTSIZE_SMALL 1
@@ -122,7 +121,6 @@ typedef struct UIScene {
   float mpc_y[50];
 
   bool world_objects_visible;
-  mat3 warp_matrix;           // transformed box -> frame.
   mat4 extrinsic_matrix;      // Last row is 0 so we can use mat4.
 
   float v_cruise;
@@ -253,12 +251,15 @@ typedef struct UIState {
   int awake_timeout;
 
   int volume_timeout;
+  int controls_timeout;
   int alert_sound_timeout;
   int speed_lim_off_timeout;
   int is_metric_timeout;
   int longitudinal_control_timeout;
   int limit_set_speed_timeout;
 
+  bool controls_seen;
+
   int status;
   bool is_metric;
   bool longitudinal_control;
@@ -458,6 +459,25 @@ sound_file* get_sound_file(AudibleAlert alert) {
   return NULL;
 }
 
+void play_alert_sound(AudibleAlert alert) {
+  sound_file* sound = get_sound_file(alert);
+  char* error = NULL;
+
+  slplay_play(sound->uri, sound->loop, &error);
+  if(error) {
+    LOGW("error playing sound: %s", error);
+  }
+}
+
+void stop_alert_sound(AudibleAlert alert) {
+  sound_file* sound = get_sound_file(alert);
+  char* error = NULL;
+
+  slplay_stop_uri(sound->uri, &error);
+  if(error) {
+    LOGW("error stopping sound: %s", error);
+  }
+}
 
 void ui_sound_init(char **error) {
   slplay_setup(error);
@@ -657,43 +677,6 @@ static void ui_init_vision(UIState *s, const VisionStreamBufs back_bufs,
   s->limit_set_speed_timeout = UI_FREQ;
 }
 
-static void ui_draw_transformed_box(UIState *s, uint32_t color) {
-  const UIScene *scene = &s->scene;
-
-  const mat3 bbt = scene->warp_matrix;
-
-  struct {
-    vec3 pos;
-    uint32_t color;
-  } verts[] = {
-    {matvecmul3(bbt, (vec3){{0.0, 0.0, 1.0,}}), color},
-    {matvecmul3(bbt, (vec3){{scene->transformed_width, 0.0, 1.0,}}), color},
-    {matvecmul3(bbt, (vec3){{scene->transformed_width, scene->transformed_height, 1.0,}}), color},
-    {matvecmul3(bbt, (vec3){{0.0, scene->transformed_height, 1.0,}}), color},
-    {matvecmul3(bbt, (vec3){{0.0, 0.0, 1.0,}}), color},
-  };
-
-  for (int i=0; i<ARRAYSIZE(verts); i++) {
-    verts[i].pos.v[0] = verts[i].pos.v[0] / verts[i].pos.v[2];
-    verts[i].pos.v[1] = s->rgb_height - verts[i].pos.v[1] / verts[i].pos.v[2];
-  }
-
-  glUseProgram(s->line_program);
-
-  mat4 out_mat = matmul(device_transform,
-                        matmul(frame_transform, s->rgb_transform));
-  glUniformMatrix4fv(s->line_transform_loc, 1, GL_TRUE, out_mat.v);
-
-  glEnableVertexAttribArray(s->line_pos_loc);
-  glVertexAttribPointer(s->line_pos_loc, 2, GL_FLOAT, GL_FALSE, sizeof(verts[0]), &verts[0].pos.v[0]);
-
-  glEnableVertexAttribArray(s->line_color_loc);
-  glVertexAttribPointer(s->line_color_loc, 4, GL_UNSIGNED_BYTE, GL_TRUE, sizeof(verts[0]), &verts[0].color);
-
-  assert(glGetError() == GL_NO_ERROR);
-  glDrawArrays(GL_LINE_STRIP, 0, ARRAYSIZE(verts));
-}
-
 // Projects a point in car to space to the corresponding point in full frame
 // image space.
 vec3 car_space_to_full_frame(const UIState *s, vec4 car_space_projective) {
@@ -1619,6 +1602,9 @@ void handle_message(UIState *s, void *which) {
     struct cereal_ControlsState datad;
     cereal_read_ControlsState(&datad, eventd.controlsState);
 
+    s->controls_timeout = 1 * UI_FREQ;
+    s->controls_seen = true;
+
     if (datad.vCruise != s->scene.v_cruise) {
       s->scene.v_cruise_update_ts = eventd.logMonoTime;
     }
@@ -1634,35 +1620,17 @@ void handle_message(UIState *s, void *which) {
 
     s->scene.decel_for_model = datad.decelForModel;
 
-    s->alert_sound_timeout = 1 * UI_FREQ;
-
     if (datad.alertSound != cereal_CarControl_HUDControl_AudibleAlert_none && datad.alertSound != s->alert_sound) {
-      char* error = NULL;
       if (s->alert_sound != cereal_CarControl_HUDControl_AudibleAlert_none) {
-        sound_file* active_sound = get_sound_file(s->alert_sound);
-        slplay_stop_uri(active_sound->uri, &error);
-        if (error) {
-          LOGW("error stopping active sound %s", error);
-        }
-      }
-
-      sound_file* sound = get_sound_file(datad.alertSound);
-      slplay_play(sound->uri, sound->loop, &error);
-      if(error) {
-        LOGW("error playing sound: %s", error);
+        stop_alert_sound(s->alert_sound);
       }
+      play_alert_sound(datad.alertSound);
 
       s->alert_sound = datad.alertSound;
       snprintf(s->alert_type, sizeof(s->alert_type), "%s", datad.alertType.str);
-    } else if ((!datad.alertSound || datad.alertSound == cereal_CarControl_HUDControl_AudibleAlert_none) && s->alert_sound != cereal_CarControl_HUDControl_AudibleAlert_none) {
-      sound_file* sound = get_sound_file(s->alert_sound);
-
-      char* error = NULL;
-
-      slplay_stop_uri(sound->uri, &error);
-      if(error) {
-        LOGW("error stopping sound: %s", error);
-      }
+    } else if ((!datad.alertSound || datad.alertSound == cereal_CarControl_HUDControl_AudibleAlert_none)
+                  && s->alert_sound != cereal_CarControl_HUDControl_AudibleAlert_none) {
+      stop_alert_sound(s->alert_sound);
       s->alert_type[0] = '\0';
       s->alert_sound = cereal_CarControl_HUDControl_AudibleAlert_none;
     }
@@ -1734,13 +1702,6 @@ void handle_message(UIState *s, void *which) {
     struct cereal_LiveCalibrationData datad;
     cereal_read_LiveCalibrationData(&datad, eventd.liveCalibration);
 
-    // should we still even have this?
-    capn_list32 warpl = datad.warpMatrix2;
-    capn_resolve(&warpl.p);  // is this a bug?
-    for (int i = 0; i < 3 * 3; i++) {
-      s->scene.warp_matrix.v[i] = capn_to_f32(capn_get32(warpl, i));
-    }
-
     capn_list32 extrinsicl = datad.extrinsicMatrix;
     capn_resolve(&extrinsicl.p);  // is this a bug?
     for (int i = 0; i < 3 * 4; i++) {
@@ -2312,18 +2273,31 @@ int main(int argc, char* argv[]) {
       set_volume(s, volume);
     }
 
-    // stop playing alert sounds if no controlsState msg for 1 second
-    if (s->alert_sound_timeout > 0) {
-      s->alert_sound_timeout--;
-    } else if (s->alert_sound != cereal_CarControl_HUDControl_AudibleAlert_none){
-      sound_file* sound = get_sound_file(s->alert_sound);
-      char* error = NULL;
+    if (s->controls_timeout > 0) {
+      s->controls_timeout--;
+    } else {
+      // stop playing alert sound
+      if ((!s->vision_connected || (s->vision_connected && s->alert_sound_timeout == 0)) &&
+            s->alert_sound != cereal_CarControl_HUDControl_AudibleAlert_none) {
+        stop_alert_sound(s->alert_sound);
+        s->alert_sound = cereal_CarControl_HUDControl_AudibleAlert_none;
+      }
 
-      slplay_stop_uri(sound->uri, &error);
-      if(error) {
-        LOGW("error stopping sound: %s", error);
+      // if visiond is still running and controlsState times out, display an alert
+      if (s->controls_seen && s->vision_connected && strcmp(s->scene.alert_text2, "Controls Unresponsive") != 0) {
+        s->scene.alert_size = ALERTSIZE_FULL;
+        update_status(s, STATUS_ALERT);
+        snprintf(s->scene.alert_text1, sizeof(s->scene.alert_text1), "%s", "TAKE CONTROL IMMEDIATELY");
+        snprintf(s->scene.alert_text2, sizeof(s->scene.alert_text2), "%s", "Controls Unresponsive");
+        ui_draw_vision_alert(s, s->scene.alert_size, s->status, s->scene.alert_text1, s->scene.alert_text2);
+
+        s->alert_sound_timeout = 2 * UI_FREQ;
+
+        s->alert_sound = cereal_CarControl_HUDControl_AudibleAlert_chimeWarningRepeat;
+        play_alert_sound(s->alert_sound);
       }
-      s->alert_sound = cereal_CarControl_HUDControl_AudibleAlert_none;
+      s->alert_sound_timeout--;
+      s->controls_seen = false;
     }
 
     read_param_bool_timeout(&s->is_metric, "IsMetric", &s->is_metric_timeout);