openpilot v0.6.3 release

old-commit-hash: d5f9caa82d

Dockerfile.openpilot

@@ -39,9 +39,9 @@ RUN cd /tmp && pipenv install --deploy --system
 
 ENV PYTHONPATH /tmp/openpilot:$PYTHONPATH
 
-RUN git clone --branch v0.6  https://github.com/commaai/openpilot-tools.git /tmp/openpilot/tools
+RUN git clone --branch v0.6.2 https://github.com/commaai/openpilot-tools.git /tmp/openpilot/tools
 RUN pip install -r /tmp/openpilot/tools/requirements.txt
-RUN pip install fastcluster==1.1.20 scipy==0.19.1
+RUN pip install fastcluster==1.1.20 scipy==0.19.1 dictdiffer==0.8.0 azure-batch==4.1.3 azure-common==1.1.16 azure-nspkg==3.0.0 azure-storage-blob==1.3.1 azure-storage-common==1.3.0 azure-storage-nspkg==3.0.0
 
 COPY ./.pylintrc /tmp/openpilot/.pylintrc
 COPY ./common /tmp/openpilot/common

README.md

@@ -72,6 +72,7 @@ Supported Cars
 | GMC<sup>3</sup>      | Acadia Denali 2018       | Adaptive Cruise      | Yes     | Yes            | 0mph             | 7mph           | Custom<sup>7</sup>|
 | Holden<sup>3</sup>   | Astra 2017               | Adaptive Cruise      | Yes     | Yes            | 0mph             | 7mph           | Custom<sup>7</sup>|
 | Honda                | Accord 2018-19           | All                  | Yes     | Stock          | 0mph             | 3mph           | Bosch             |
+| Honda                | Accord Hybrid 2018-19    | All                  | Yes     | Stock          | 0mph             | 3mph           | Bosch             |
 | Honda                | Civic Sedan/Coupe 2016-18| Honda Sensing        | Yes     | Yes            | 0mph             | 12mph          | Nidec             |
 | Honda                | Civic Sedan/Coupe 2019   | Honda Sensing        | Yes     | Stock          | 0mph             | 2mph           | Bosch             |
 | Honda                | Civic Hatchback 2017-19  | Honda Sensing        | Yes     | Stock          | 0mph             | 12mph          | Bosch             |
@@ -98,18 +99,21 @@ Supported Cars
 | Toyota               | Avalon 2016              | TSS-P                | Yes     | Yes<sup>2</sup>| 20mph<sup>1</sup>| 0mph           | Toyota            |
 | Toyota               | Avalon 2017-18           | All                  | Yes     | Yes<sup>2</sup>| 20mph<sup>1</sup>| 0mph           | Toyota            |
 | Toyota               | Camry 2018-19            | All                  | Yes     | Stock          | 0mph<sup>5</sup> | 0mph           | Toyota            |
+| Toyota               | Camry Hybrid 2018-19     | All                  | Yes     | Stock          | 0mph<sup>5</sup> | 0mph           | Toyota            |
 | Toyota               | C-HR 2017-19             | All                  | Yes     | Stock          | 0mph             | 0mph           | Toyota            |
+| Toyota               | C-HR Hybrid 2017-19      | All                  | Yes     | Stock          | 0mph             | 0mph           | Toyota            |
 | Toyota               | Corolla 2017-19          | All                  | Yes     | Yes<sup>2</sup>| 20mph<sup>1</sup>| 0mph           | Toyota            |
 | Toyota               | Corolla 2020             | All                  | Yes     | Yes            | 0mph             | 0mph           | Toyota            |
 | Toyota               | Corolla Hatchback 2019   | All                  | Yes     | Yes            | 0mph             | 0mph           | Toyota            |
 | Toyota               | Highlander 2017-19       | All                  | Yes     | Yes<sup>2</sup>| 0mph             | 0mph           | Toyota            |
-| Toyota               | Highlander Hybrid 2018   | All                  | Yes     | Yes<sup>2</sup>| 0mph             | 0mph           | Toyota            |
+| Toyota               | Highlander Hybrid 2017-19| All                  | Yes     | Yes<sup>2</sup>| 0mph             | 0mph           | Toyota            |
 | Toyota               | Prius 2016               | TSS-P                | Yes     | Yes<sup>2</sup>| 0mph             | 0mph           | Toyota            |
 | Toyota               | Prius 2017-19            | All                  | Yes     | Yes<sup>2</sup>| 0mph             | 0mph           | Toyota            |
 | Toyota               | Prius Prime 2017-19      | All                  | Yes     | Yes<sup>2</sup>| 0mph             | 0mph           | Toyota            |
 | Toyota               | Rav4 2016                | TSS-P                | Yes     | Yes<sup>2</sup>| 20mph<sup>1</sup>| 0mph           | Toyota            |
 | Toyota               | Rav4 2017-18             | All                  | Yes     | Yes<sup>2</sup>| 20mph<sup>1</sup>| 0mph           | Toyota            |
 | Toyota               | Rav4 2019                | All                  | Yes     | Yes            | 0mph             | 0mph           | Toyota            |
+| Toyota               | Rav4 Hybrid 2016         | TSS-P                | Yes     | Yes<sup>2</sup>| 0mph             | 0mph           | Toyota            |
 | Toyota               | Rav4 Hybrid 2017-18      | All                  | Yes     | Yes<sup>2</sup>| 0mph             | 0mph           | Toyota            |
 | Toyota               | Sienna 2018              | All                  | Yes     | Yes<sup>2</sup>| 0mph             | 0mph           | Toyota            |
 

RELEASES.md

@@ -1,3 +1,14 @@
+Version 0.6.3 (2019-08-12)
+========================
+ * Alert sounds from EON: requires NEOS update
+ * Improve driver monitoring: eye tracking and improved awareness logic
+ * Improve path prediction with new driving model
+ * Improve lane positioning with wide lanes and exits
+ * Improve lateral control on RAV4
+ * Slow down for turns using model
+ * Open sourced regression test to verify outputs against reference logs
+ * Open sourced regression test to sanity check all car models
+
 Version 0.6.2 (2019-07-29)
 ========================
  * New driving model!

apk/ai.comma.plus.offroad.apk

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:3bf24d5dd606d364e339c8823a5b71b0a5271716b551a187ca8a8ea457a275d0
+oid sha256:9eba15d8e77847b242a83fbd43dd67a5bd5d0c6d76f4f11f6fd6820621f69771
-size 17042107
+size 17096653

common/api/__init__.py

@@ -1,7 +1,26 @@
+import jwt
 import requests
+from datetime import datetime, timedelta
 
 from selfdrive.version import version
 
+class Api(object):
+  def __init__(self, dongle_id, private_key):
+    self.dongle_id = dongle_id
+    self.private_key = private_key
+
+  def get(self, *args, **kwargs):
+    return self.request('GET', *args, **kwargs)
+
+  def post(self, *args, **kwargs):
+    return self.request('POST', *args, **kwargs)
+
+  def request(self, method, endpoint, timeout=None, access_token=None, **params):
+    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')
+
 def api_get(endpoint, method='GET', timeout=None, access_token=None, **params):
   backend = "https://api.commadotai.com/"
 

installer/updater/Makefile

@@ -31,7 +31,9 @@ FRAMEBUFFER_LIBS = -lutils -lgui -lEGL
 .PHONY: all
 all: updater
 
-OBJS = courbd.ttf.o \
+OBJS = opensans_regular.ttf.o \
+			 opensans_semibold.ttf.o \
+			 opensans_bold.ttf.o \
        ../../selfdrive/common/touch.o \
        ../../selfdrive/common/framebuffer.o \
        $(PHONELIBS)/json11/json11.o \
@@ -50,7 +52,15 @@ updater: updater.o $(OBJS)
     -lcutils -lm -llog
 	strip updater
 
-courbd.ttf.o: ../../selfdrive/assets/courbd.ttf
+opensans_regular.ttf.o: ../../selfdrive/assets/fonts/opensans_regular.ttf
+	@echo "[ bin2o ] $@"
+	cd '$(dir $<)' && ld -r -b binary '$(notdir $<)' -o '$(abspath $@)'
+
+opensans_bold.ttf.o: ../../selfdrive/assets/fonts/opensans_bold.ttf
+	@echo "[ bin2o ] $@"
+	cd '$(dir $<)' && ld -r -b binary '$(notdir $<)' -o '$(abspath $@)'
+
+opensans_semibold.ttf.o: ../../selfdrive/assets/fonts/opensans_semibold.ttf
 	@echo "[ bin2o ] $@"
 	cd '$(dir $<)' && ld -r -b binary '$(notdir $<)' -o '$(abspath $@)'
 

installer/updater/update.json

@@ -1,7 +1,7 @@
 {
-  "ota_url": "https://commadist.azureedge.net/neosupdate/ota-signed-c992abb59cbaf6588f51055db52db619061107851773fc8480acb8bb5d77a28f.zip",
+  "ota_url": "https://commadist.azureedge.net/neosupdate/ota-signed-4db25072191d24e204a816d73ac9e8c727822a26ed3baf01ecae18167fa2eb11.zip",
-  "ota_hash": "c992abb59cbaf6588f51055db52db619061107851773fc8480acb8bb5d77a28f",
+  "ota_hash": "4db25072191d24e204a816d73ac9e8c727822a26ed3baf01ecae18167fa2eb11",
-  "recovery_url": "https://commadist.azureedge.net/neosupdate/recovery-af099a84cfd7b91266090779238ac358278948dcde2dcfa0fbca6e8397366f0a.img",
+  "recovery_url": "https://commadist.azureedge.net/neosupdate/recovery-31ef14206d3102edf18fb7417ef32ba2d9f37dd2f4443e234c374a70d1bf4662.img",
   "recovery_len": 15136044,
-  "recovery_hash": "af099a84cfd7b91266090779238ac358278948dcde2dcfa0fbca6e8397366f0a"
+  "recovery_hash": "31ef14206d3102edf18fb7417ef32ba2d9f37dd2f4443e234c374a70d1bf4662"
 }

installer/updater/updater

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:743ba86c3b4d643272c11eba28e2b0a8824c542caa613a94ff6a2f01c37e54a7
+oid sha256:da43154c2563cfda1af50018a412a6105338c4fafc021f8b3c7442fcec514d44
-size 2501656
+size 2464536

installer/updater/updater.cc

@@ -43,8 +43,12 @@ const char *manifest_url = MANIFEST_URL_EON;
 
 #define UPDATE_DIR "/data/neoupdate"
 
-extern const uint8_t bin_courbd[] asm("_binary_courbd_ttf_start");
+extern const uint8_t bin_opensans_regular[] asm("_binary_opensans_regular_ttf_start");
-extern const uint8_t bin_courbd_end[] asm("_binary_courbd_ttf_end");
+extern const uint8_t bin_opensans_regular_end[] asm("_binary_opensans_regular_ttf_end");
+extern const uint8_t bin_opensans_semibold[] asm("_binary_opensans_semibold_ttf_start");
+extern const uint8_t bin_opensans_semibold_end[] asm("_binary_opensans_semibold_ttf_end");
+extern const uint8_t bin_opensans_bold[] asm("_binary_opensans_bold_ttf_start");
+extern const uint8_t bin_opensans_bold_end[] asm("_binary_opensans_bold_ttf_end");
 
 namespace {
 
@@ -148,7 +152,9 @@ struct Updater {
 
   FramebufferState *fb = NULL;
   NVGcontext *vg = NULL;
-  int font;
+  int font_regular;
+  int font_semibold;
+  int font_bold;
 
   std::thread update_thread_handle;
 
@@ -182,14 +188,21 @@ struct Updater {
 
     vg = nvgCreateGLES3(NVG_ANTIALIAS | NVG_STENCIL_STROKES | NVG_DEBUG);
     assert(vg);
-    font = nvgCreateFontMem(vg, "courbd", (unsigned char*)bin_courbd, (bin_courbd_end - bin_courbd), 0);
-    assert(font >= 0);
 
-    b_w = 600;
+    font_regular = nvgCreateFontMem(vg, "opensans_regular", (unsigned char*)bin_opensans_regular, (bin_opensans_regular_end - bin_opensans_regular), 0);
+    assert(font_regular >= 0);
+
+    font_semibold = nvgCreateFontMem(vg, "opensans_semibold", (unsigned char*)bin_opensans_semibold, (bin_opensans_semibold_end - bin_opensans_semibold), 0);
+    assert(font_semibold >= 0);
+
+    font_bold = nvgCreateFontMem(vg, "opensans_bold", (unsigned char*)bin_opensans_bold, (bin_opensans_bold_end - bin_opensans_bold), 0);
+    assert(font_bold >= 0);
+
+    b_w = 640;
     balt_x = 200;
     b_x = fb_w-b_w-200;
-    b_y = 700;
+    b_y = 720;
-    b_h = 250;
+    b_h = 220;
 
     state = CONFIRMATION;
 
@@ -286,14 +299,14 @@ struct Updater {
   std::string stage_download(std::string url, std::string hash, std::string name) {
     std::string out_fn = UPDATE_DIR "/" + util::base_name(url);
 
-    set_progress("downloading " + name + "...");
+    set_progress("Downloading " + name + "...");
     bool r = download_file(url, out_fn);
     if (!r) {
       set_error("failed to download " + name);
       return "";
     }
 
-    set_progress("verifying " + name + "...");
+    set_progress("Verifying " + name + "...");
     std::string fn_hash = sha256_file(out_fn);
     printf("got %s hash: %s\n", name.c_str(), hash.c_str());
     if (fn_hash != hash) {
@@ -323,7 +336,7 @@ struct Updater {
 
     const int EON = (access("/EON", F_OK) != -1);
 
-    set_progress("finding latest version...");
+    set_progress("Finding latest version...");
     std::string manifest_s;
     if (EON) {
       manifest_s = download_string(curl, manifest_url);
@@ -364,10 +377,10 @@ struct Updater {
 
     std::string recovery_fn;
     if (recovery_url.empty() || recovery_hash.empty() || recovery_len == 0) {
-      set_progress("skipping recovery flash...");
+      set_progress("Skipping recovery flash...");
     } else {
       // only download the recovery if it differs from what's flashed
-      set_progress("checking recovery...");
+      set_progress("Checking recovery...");
       std::string existing_recovery_hash = sha256_file(RECOVERY_DEV, recovery_len);
       printf("existing recovery hash: %s\n", existing_recovery_hash.c_str());
 
@@ -393,7 +406,7 @@ struct Updater {
 
     if (!recovery_fn.empty()) {
       // flash recovery
-      set_progress("flashing recovery...");
+      set_progress("Flashing recovery...");
 
       FILE *flash_file = fopen(recovery_fn.c_str(), "rb");
       if (!flash_file) {
@@ -427,7 +440,7 @@ struct Updater {
       fclose(recovery_dev);
       fclose(flash_file);
 
-      set_progress("verifying flash...");
+      set_progress("Verifying flash...");
       std::string new_recovery_hash = sha256_file(RECOVERY_DEV, recovery_len);
       printf("new recovery hash: %s\n", new_recovery_hash.c_str());
 
@@ -447,7 +460,7 @@ struct Updater {
     fprintf(cmd_file, "--update_package=%s\n", ota_fn.c_str());
     fclose(cmd_file);
 
-    set_progress("rebooting");
+    set_progress("Rebooting");
 
     // remove the continue.sh so we come back into the setup.
     // maybe we should go directly into the installer, but what if we don't come back with internet? :/
@@ -462,25 +475,32 @@ struct Updater {
     // set_error("failed to reboot into recovery");
   }
 
-  void draw_ack_screen(const char *message, const char *button, const char *altbutton) {
+  void draw_ack_screen(const char *title, const char *message, const char *button, const char *altbutton) {
-    nvgFontSize(vg, 96.0f);
     nvgFillColor(vg, nvgRGBA(255,255,255,255));
-    nvgTextAlign(vg, NVG_ALIGN_LEFT | NVG_ALIGN_BASELINE);
+    nvgTextAlign(vg, NVG_ALIGN_CENTER | NVG_ALIGN_BASELINE);
-    nvgTextBox(vg, 50, 100, fb_w-100, message, NULL);
+
+    nvgFontFace(vg, "opensans_bold");
+    nvgFontSize(vg, 120.0f);
+    nvgTextBox(vg, 110, 220, fb_w-240, title, NULL);
+
+    nvgFontFace(vg, "opensans_regular");
+    nvgFontSize(vg, 86.0f);
+    nvgTextBox(vg, 130, 380, fb_w-260, message, NULL);
 
     // draw button
     if (button) {
       nvgBeginPath(vg);
-      nvgFillColor(vg, nvgRGBA(0, 0, 0, 255));
+      nvgFillColor(vg, nvgRGBA(8, 8, 8, 255));
       nvgRoundedRect(vg, b_x, b_y, b_w, b_h, 20);
       nvgFill(vg);
 
       nvgFillColor(vg, nvgRGBA(255, 255, 255, 255));
+      nvgFontFace(vg, "opensans_semibold");
       nvgTextAlign(vg, NVG_ALIGN_CENTER | NVG_ALIGN_MIDDLE);
       nvgText(vg, b_x+b_w/2, b_y+b_h/2, button, NULL);
 
       nvgBeginPath(vg);
-      nvgStrokeColor(vg, nvgRGBA(255, 255, 255, 255));
+      nvgStrokeColor(vg, nvgRGBA(255, 255, 255, 50));
       nvgStrokeWidth(vg, 5);
       nvgRoundedRect(vg, b_x, b_y, b_w, b_h, 20);
       nvgStroke(vg);
@@ -489,16 +509,17 @@ struct Updater {
     // draw button
     if (altbutton) {
       nvgBeginPath(vg);
-      nvgFillColor(vg, nvgRGBA(0, 0, 0, 255));
+      nvgFillColor(vg, nvgRGBA(8, 8, 8, 255));
       nvgRoundedRect(vg, balt_x, b_y, b_w, b_h, 20);
       nvgFill(vg);
 
       nvgFillColor(vg, nvgRGBA(255, 255, 255, 255));
+      nvgFontFace(vg, "opensans_semibold");
       nvgTextAlign(vg, NVG_ALIGN_CENTER | NVG_ALIGN_MIDDLE);
       nvgText(vg, balt_x+b_w/2, b_y+b_h/2, altbutton, NULL);
 
       nvgBeginPath(vg);
-      nvgStrokeColor(vg, nvgRGBA(255, 255, 255, 255));
+      nvgStrokeColor(vg, nvgRGBA(255, 255, 255, 50));
       nvgStrokeWidth(vg, 5);
       nvgRoundedRect(vg, balt_x, b_y, b_w, b_h, 20);
       nvgStroke(vg);
@@ -510,23 +531,27 @@ struct Updater {
     nvgFontSize(vg, 64.0f);
     nvgFillColor(vg, nvgRGBA(255,255,255,255));
     nvgTextAlign(vg, NVG_ALIGN_CENTER | NVG_ALIGN_BASELINE);
-    nvgTextBox(vg, 0, 700, fb_w, progress_text.c_str(), NULL);
+    nvgFontFace(vg, "opensans_bold");
+    nvgFontSize(vg, 86.0f);
+    nvgTextBox(vg, 0, 380, fb_w, progress_text.c_str(), NULL);
 
     // draw progress bar
     {
-      int progress_width = 800;
+      int progress_width = 1000;
       int progress_x = fb_w/2-progress_width/2;
-      int progress_y = 768;
+      int progress_y = 520;
-      int progress_height = 15;
+      int progress_height = 50;
 
-      int powerprompt_y = 512;
+      int powerprompt_y = 312;
-      nvgText(vg, fb_w/2, powerprompt_y, "Ensure EON is connected to power", NULL);
+      nvgFontFace(vg, "opensans_regular");
+      nvgFontSize(vg, 64.0f);
+      nvgText(vg, fb_w/2, 740, "Ensure EON is connected to power.", NULL);
 
       NVGpaint paint = nvgBoxGradient(
           vg, progress_x + 1, progress_y + 1,
-          progress_width - 2, progress_height, 3, 4, nvgRGB(0, 32, 0), nvgRGB(0, 92, 0));
+          progress_width - 2, progress_height, 3, 4, nvgRGB(27, 27, 27), nvgRGB(27, 27, 27));
       nvgBeginPath(vg);
-      nvgRoundedRect(vg, progress_x, progress_y, progress_width, progress_height, 3);
+      nvgRoundedRect(vg, progress_x, progress_y, progress_width, progress_height, 12);
       nvgFillPaint(vg, paint);
       nvgFill(vg);
 
@@ -536,12 +561,12 @@ struct Updater {
       paint = nvgBoxGradient(
           vg, progress_x, progress_y,
           bar_pos+1.5f, progress_height-1, 3, 4,
-          nvgRGB(220, 100, 0), nvgRGB(128, 100, 0));
+          nvgRGB(245, 245, 245), nvgRGB(105, 105, 105));
 
       nvgBeginPath(vg);
       nvgRoundedRect(
           vg, progress_x+1, progress_y+1,
-          bar_pos, progress_height-2, 3);
+          bar_pos, progress_height-2, 12);
       nvgFillPaint(vg, paint);
       nvgFill(vg);
     }
@@ -554,16 +579,16 @@ struct Updater {
 
     switch (state) {
     case CONFIRMATION:
-      draw_ack_screen("An upgrade to NEOS is required.\n\n"
+      draw_ack_screen("An update to NEOS is required.",
-                      "Your device will now be reset and upgraded. You may want to connect to wifi as download is around 1 GB\nData on device shouldn't be lost.",
+                      "Your device will now be reset and upgraded. You may want to connect to wifi as download is around 1 GB. Existing data on device should not be lost.",
-                      "continue",
+                      "Continue",
-                      "wifi");
+                      "Connect to WiFi");
       break;
     case RUNNING:
       draw_progress_screen();
       break;
     case ERROR:
-      draw_ack_screen(("ERROR: " + error_text + "\n\nYou will need to retry").c_str(), NULL, "exit");
+      draw_ack_screen("There was an error.", ("ERROR: " + error_text + "\n\nYou will need to retry").c_str(), NULL, "exit");
       break;
     }
 
@@ -604,9 +629,17 @@ struct Updater {
     while (!do_exit) {
       ui_update();
 
-      glClearColor(0.19, 0.09, 0.2, 1.0);
+      glClearColor(0.08, 0.08, 0.08, 1.0);
       glClear(GL_STENCIL_BUFFER_BIT | GL_COLOR_BUFFER_BIT);
 
+      // background
+      nvgBeginPath(vg);
+      NVGpaint bg = nvgLinearGradient(vg, fb_w, 0, fb_w, fb_h,
+        nvgRGBA(0, 0, 0, 0), nvgRGBA(0, 0, 0, 255));
+      nvgFillPaint(vg, bg);
+      nvgRect(vg, 0, 0, fb_w, fb_h);
+      nvgFill(vg);
+
       glEnable(GL_BLEND);
       glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
 

models/driving_model.dlc

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:32aef4d710c994e0c8a019b64eb2bd495f493be0dc0daa9f312f7dfeeb7f1568
+oid sha256:f5fb0036eaaa6136b8b605c24ab255de45b14ce827af538e15c47a04a6b200af
-size 14761531
+size 14438998

models/monitoring_model.dlc

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:eb047afc34b4b5b9888dc1dfc796d4f83fc695734decac5f6a6057a70e468623
+oid sha256:9fdd514ad8a38876468adf9fa106f511150c077223c13eb1cc3ae939bc414564
-size 544528
+size 601956

run_docker_tests.sh

@@ -3,7 +3,7 @@ set -e
 
 docker build -t tmppilot -f Dockerfile.openpilot .
 
-docker run --rm tmppilot /bin/sh -c 'cd /tmp/openpilot/selfdrive/test/ && ./test_fingerprints.py'
+docker run --rm tmppilot /bin/sh -c 'cd /tmp/openpilot/ && make -C cereal && cd /tmp/openpilot/selfdrive/test/ && ./test_fingerprints.py'
 docker run --rm tmppilot /bin/sh -c 'cd /tmp/openpilot/ && pyflakes $(find . -iname "*.py" | grep -vi "^\./pyextra.*" | grep -vi "^\./panda" | grep -vi "^\./tools")'
 docker run --rm tmppilot /bin/sh -c 'cd /tmp/openpilot/ && pylint $(find . -iname "*.py" | grep -vi "^\./pyextra.*" | grep -vi "^\./panda" | grep -vi "^\./tools"); exit $(($? & 3))'
 docker run --rm tmppilot /bin/sh -c 'cd /tmp/openpilot/ && make -C cereal && python -m unittest discover common'
@@ -12,3 +12,5 @@ docker run --rm tmppilot /bin/sh -c 'cd /tmp/openpilot/ && make -C cereal && pyt
 docker run --rm tmppilot /bin/sh -c 'cd /tmp/openpilot/ && make -C cereal && python -m unittest discover selfdrive/controls'
 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'

selfdrive/assets/Roboto-Bold.ttf

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

selfdrive/assets/sounds/disengaged.wav

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:c898025ae57cbc55d29af9381fe65fd2db4704beb46c606411c4522d059bfe0b
+oid sha256:f888f84627b4b69395cdf3f8bedf7a6a50c13253b30b7ddc2eda010c8be16b66
-size 428188
+size 40662

selfdrive/assets/sounds/engaged.wav

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:bd26959d1a73071764378885fbb1a2941cfd4a15650361a5b6fbeccf81900def
+oid sha256:113afc45a281e491a771bdd749f1375b96b174e9c74a8dff5ab1cd83f3cad406
-size 159998
+size 40654

selfdrive/assets/sounds/error.wav

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:6c0cf1b6fcdb5489146b2973aed01c4deb2bed34d9610e39b8dc3596d6e1e9af
+oid sha256:301eef864fa7b306527828bb9e8295208c08c24382026391a9206b19aca58a4e
-size 71030
+size 40650

selfdrive/assets/sounds/warning_1.wav

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:d39d45560063a9b3f0b2c473cda5ea1e052b38ee3fd7dcbd4465043b37f661ed
+oid sha256:9cffe1d8cd2dec399fb3fce5ce2833e0405bd44191d3a24d6085592cd8d31dd5
-size 128798
+size 21468

selfdrive/assets/sounds/warning_2.wav

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:8249c685cab9896799290f28a53cc3751dd3fbf31c300e03fd039cd129e6c03c
+oid sha256:62580fd60d5784505f4f7697e5a5011cf0ae1f3c92ed1104700159bfbcd7de3e
-size 119446
+size 52800

selfdrive/athena/athenad.py

@@ -21,6 +21,7 @@ from selfdrive.loggerd.config import ROOT
 
 import selfdrive.crash as crash
 import selfdrive.messaging as messaging
+from common.api import Api
 from common.params import Params
 from selfdrive.services import service_list
 from selfdrive.swaglog import cloudlog
@@ -225,19 +226,21 @@ def backoff(retries):
 def main(gctx=None):
   params = Params()
   dongle_id = params.get("DongleId")
-  access_token = params.get("AccessToken")
+  ws_uri = ATHENA_HOST + "/ws/v2/" + dongle_id
-  ws_uri = ATHENA_HOST + "/ws/" + dongle_id
 
   crash.bind_user(id=dongle_id)
   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)
+
   conn_retries = 0
   while 1:
     try:
       print("connecting to %s" % ws_uri)
       ws = create_connection(ws_uri,
-                             cookie="jwt=" + access_token,
+                             cookie="jwt=" + api.get_token(),
                              enable_multithread=True)
       ws.settimeout(1)
       conn_retries = 0

selfdrive/can/tests/test_packer_honda.py

@@ -23,11 +23,10 @@ class TestPackerMethods(unittest.TestCase):
       pump_on = (random.randint(0, 2) % 2 == 0)
       pcm_override = (random.randint(0, 2) % 2 == 0)
       pcm_cancel_cmd = (random.randint(0, 2) % 2 == 0)
-      chime = random.randint(0, 65536)
       fcw = random.randint(0, 65536)
       idx = random.randint(0, 65536)
-      m_old = hondacan.create_brake_command(self.honda_cp_old, apply_brake, pump_on, pcm_override, pcm_cancel_cmd, chime, fcw, idx, car_fingerprint, is_panda_black)
+      m_old = hondacan.create_brake_command(self.honda_cp_old, apply_brake, pump_on, pcm_override, pcm_cancel_cmd, fcw, idx, car_fingerprint, is_panda_black)
-      m = hondacan.create_brake_command(self.honda_cp, apply_brake, pump_on, pcm_override, pcm_cancel_cmd, chime, fcw, idx, car_fingerprint, is_panda_black)
+      m = hondacan.create_brake_command(self.honda_cp, apply_brake, pump_on, pcm_override, pcm_cancel_cmd, fcw, idx, car_fingerprint, is_panda_black)
       self.assertEqual(m_old, m)
 
       apply_steer = (random.randint(0, 2) % 2 == 0)
@@ -39,8 +38,7 @@ class TestPackerMethods(unittest.TestCase):
 
       pcm_speed = random.randint(0, 65536)
       hud = HUDData(random.randint(0, 65536), random.randint(0, 65536), 1, random.randint(0, 65536),
-              0xc1, random.randint(0, 65536), random.randint(0, 65536), random.randint(0, 65536),
+              0xc1, random.randint(0, 65536), random.randint(0, 65536), random.randint(0, 65536), random.randint(0, 65536))
-              random.randint(0, 65536), random.randint(0, 65536), random.randint(0, 65536))
       idx = random.randint(0, 65536)
       is_metric = (random.randint(0, 2) % 2 == 0)
       m_old = hondacan.create_ui_commands(self.honda_cp_old, pcm_speed, hud, car_fingerprint, is_metric, idx, is_panda_black)

selfdrive/can/tests/test_packer_toyota.py

@@ -47,36 +47,32 @@ class TestPackerMethods(unittest.TestCase):
       self.assertEqual(m_old, m)
 
       steer = (random.randint(0, 2) % 2 == 0)
-      sound1 = random.randint(1, 65536)
-      sound2 = 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, sound1, sound2, left_line, right_line, left_lane_depart, right_lane_depart)
+      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, sound1, sound2, 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)
       self.assertEqual(m_old, m)
 
   def test_performance(self):
     n1 = sec_since_boot()
     recursions = 100000
     steer = (random.randint(0, 2) % 2 == 0)
-    sound1 = random.randint(1, 65536)
-    sound2 = 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, sound1, sound2, left_line, right_line, left_lane_depart, right_lane_depart)
+      create_ui_command(self.cp_old, steer, 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, sound1, sound2, left_line, right_line, left_lane_depart, right_lane_depart)
+      create_ui_command(self.cp, steer, 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/car/chrysler/carcontroller.py

@@ -1,14 +1,9 @@
-from cereal import car
 from selfdrive.car import apply_toyota_steer_torque_limits
 from selfdrive.car.chrysler.chryslercan import create_lkas_hud, create_lkas_command, \
-                                               create_wheel_buttons, \
+                                               create_wheel_buttons
-                                               create_chimes
 from selfdrive.car.chrysler.values import ECU, CAR
 from selfdrive.can.packer import CANPacker
 
-AudibleAlert = car.CarControl.HUDControl.AudibleAlert
-LOUD_ALERTS = [AudibleAlert.chimeWarning1, AudibleAlert.chimeWarning2, AudibleAlert.chimeWarningRepeat]
-
 class SteerLimitParams:
   STEER_MAX = 261         # 262 faults
   STEER_DELTA_UP = 3      # 3 is stock. 100 is fine. 200 is too much it seems
@@ -27,7 +22,6 @@ class CarController(object):
     self.hud_count = 0
     self.car_fingerprint = car_fingerprint
     self.alert_active = False
-    self.send_chime = False
     self.gone_fast_yet = False
 
     self.fake_ecus = set()
@@ -37,7 +31,7 @@ class CarController(object):
     self.packer = CANPacker(dbc_name)
 
 
-  def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, hud_alert, audible_alert):
+  def update(self, enabled, CS, frame, 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:
@@ -62,19 +56,10 @@ class CarController(object):
 
     self.apply_steer_last = apply_steer
 
-    if audible_alert in LOUD_ALERTS:
-      self.send_chime = True
-
     can_sends = []
 
     #*** control msgs ***
 
-    if self.send_chime:
-      new_msg = create_chimes(AudibleAlert)
-      can_sends.append(new_msg)
-      if audible_alert not in LOUD_ALERTS:
-        self.send_chime = False
-
     if pcm_cancel_cmd:
       # TODO: would be better to start from frame_2b3
       new_msg = create_wheel_buttons(self.ccframe)

selfdrive/car/chrysler/chryslercan.py

@@ -2,7 +2,6 @@ from cereal import car
 
 
 VisualAlert = car.CarControl.HUDControl.VisualAlert
-AudibleAlert = car.CarControl.HUDControl.AudibleAlert
 
 def calc_checksum(data):
   """This function does not want the checksum byte in the input data.
@@ -94,15 +93,6 @@ def create_lkas_command(packer, apply_steer, moving_fast, frame):
   return packer.make_can_msg("LKAS_COMMAND", 0, values)
 
 
-def create_chimes(audible_alert):
-  # '0050' nothing, chime '4f55'
-  if audible_alert == AudibleAlert.none:
-    msg = '0050'.decode('hex')
-  else:
-    msg = '4f55'.decode('hex')
-  return make_can_msg(0x339, msg)
-
-
 def create_wheel_buttons(frame):
   # WHEEL_BUTTONS (571) Message sent to cancel ACC.
   start = [0x01]  # acc cancel set

selfdrive/car/chrysler/chryslercan_test.py

@@ -3,7 +3,6 @@ from selfdrive.can.packer import CANPacker
 
 from cereal import car
 VisualAlert = car.CarControl.HUDControl.VisualAlert
-AudibleAlert = car.CarControl.HUDControl.AudibleAlert
 
 import unittest
 

selfdrive/car/chrysler/interface.py

@@ -238,7 +238,6 @@ class CarInterface(object):
 
     self.frame = self.CS.frame
     can_sends = self.CC.update(c.enabled, self.CS, self.frame,
-                               c.actuators, c.cruiseControl.cancel, c.hudControl.visualAlert,
+                               c.actuators, c.cruiseControl.cancel, c.hudControl.visualAlert)
-                               c.hudControl.audibleAlert)
 
     return can_sends

selfdrive/car/chrysler/values.py

@@ -24,7 +24,7 @@ FINGERPRINTS = {
     {168: 8, 257: 5, 258: 8, 264: 8, 268: 8, 270: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 291: 8, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 515: 7, 516: 7, 517: 7, 518: 7, 520: 8, 528: 8, 532: 8, 542: 8, 544: 8, 557: 8, 559: 8, 560: 4, 564: 4, 571: 3, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 653: 8, 654: 8, 655: 8, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 678: 8, 680: 8, 701: 8, 704: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 737: 8, 746: 5, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 782: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 808: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 878: 8, 882: 8, 897: 8, 908: 8, 924: 3, 926: 3, 929: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 956: 8, 958: 8, 959: 8, 969: 4, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1082: 8, 1083: 8, 1098: 8, 1100: 8, 1216: 8, 1218: 8, 1220: 8, 1225: 8, 1235: 8, 1242: 8, 1246: 8, 1250: 8, 1284: 8, 1537: 8, 1538: 8, 1562: 8, 1568: 8, 1856: 8, 1858: 8, 1860: 8, 1865: 8, 1875: 8, 1882: 8, 1886: 8, 1890: 8, 1892: 8, 2016: 8, 2024: 8},
   ],
   CAR.PACIFICA_2018: [
-    {55: 8, 257: 5, 258: 8, 264: 8, 268: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 416: 7, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 520: 8, 528: 8, 532: 8, 544: 8, 557: 8, 559: 8, 560: 4, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 678: 8, 680: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 746: 5, 752: 2, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 882: 8, 897: 8, 924: 8, 926: 3, 937: 8, 947: 8, 948: 8, 969: 4, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1098: 8, 1100: 8},
+    {55: 8, 257: 5, 258: 8, 264: 8, 268: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 416: 7, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 516: 7, 517: 7, 520: 8, 524: 8, 526: 6, 528: 8, 532: 8, 542: 8, 544: 8, 557: 8, 559: 8, 560: 4, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 656: 4, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 678: 8, 680: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 746: 5, 752: 2, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 808: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 882: 8, 897: 8, 924: 8, 926: 3, 937: 8, 947: 8, 948: 8, 969: 4, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1098: 8, 1100: 8, 1537: 8, 1538: 8, 1562: 8},
   ],
   CAR.PACIFICA_2018_HYBRID: [
     {68: 8, 168: 8, 257: 5, 258: 8, 264: 8, 268: 8, 270: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 291: 8, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 520: 8, 528: 8, 532: 8, 544: 8, 557: 8, 559: 8, 560: 4, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 653: 8, 654: 8, 655: 8, 660: 8, 669: 3, 671: 8, 672: 8, 680: 8, 701: 8, 704: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 736: 8, 737: 8, 746: 5, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 782: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 808: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 878: 8, 882: 8, 897: 8, 908: 8, 924: 8, 926: 3, 929: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 958: 8, 959: 8, 969: 4, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1082: 8, 1083: 8, 1098: 8, 1100: 8},

selfdrive/car/gm/carcontroller.py

@@ -72,7 +72,6 @@ class CarController(object):
   def __init__(self, canbus, car_fingerprint):
     self.pedal_steady = 0.
     self.start_time = 0.
-    self.chime = 0
     self.steer_idx = 0
     self.apply_steer_last = 0
     self.car_fingerprint = car_fingerprint
@@ -87,7 +86,7 @@ class CarController(object):
     self.packer_ch = CANPacker(DBC[car_fingerprint]['chassis'])
 
   def update(self, enabled, CS, frame, actuators, \
-             hud_v_cruise, hud_show_lanes, hud_show_car, chime, chime_cnt, hud_alert):
+             hud_v_cruise, hud_show_lanes, hud_show_car, hud_alert):
 
     P = self.params
 
@@ -183,21 +182,4 @@ class CarController(object):
         can_sends.append(gmcan.create_lka_icon_command(canbus.sw_gmlan, lka_active, lka_critical, steer))
         self.lka_icon_status_last = lka_icon_status
 
-    # Send chimes
-    if self.chime != chime:
-      duration = 0x3c
-
-      # There is no 'repeat forever' chime command
-      # TODO: Manage periodic re-issuing of chime command
-      # and chime cancellation
-      if chime_cnt == -1:
-        chime_cnt = 10
-
-      if chime != 0:
-        can_sends.append(gmcan.create_chime_command(canbus.sw_gmlan, chime, duration, chime_cnt))
-
-      # If canceling a repeated chime, cancel command must be
-      # issued for the same chime type and duration
-      self.chime = chime
-
     return can_sends

selfdrive/car/gm/gmcan.py

@@ -130,10 +130,6 @@ def create_adas_accelerometer_speed_status(bus, speed_ms, idx):
 def create_adas_headlights_status(bus):
   return [0x310, 0, "\x42\x04", bus]
 
-def create_chime_command(bus, chime_type, duration, repeat_cnt):
-  dat = [chime_type, duration, repeat_cnt, 0xff, 0]
-  return [0x10400060, 0, "".join(map(chr, dat)), bus]
-
 def create_lka_icon_command(bus, active, critical, steer):
   if active and steer == 1:
     if critical:

selfdrive/car/gm/interface.py

@@ -4,7 +4,7 @@ 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
-from selfdrive.car.gm.values import DBC, CAR, STOCK_CONTROL_MSGS, AUDIO_HUD, \
+from selfdrive.car.gm.values import DBC, CAR, STOCK_CONTROL_MSGS, \
                                     SUPERCRUISE_CARS, AccState
 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
@@ -325,8 +325,6 @@ class CarInterface(object):
     if hud_v_cruise > 70:
       hud_v_cruise = 0
 
-    chime, chime_count = AUDIO_HUD[c.hudControl.audibleAlert.raw]
-
     # For Openpilot, "enabled" includes pre-enable.
     # In GM, PCM faults out if ACC command overlaps user gas.
     enabled = c.enabled and not self.CS.user_gas_pressed
@@ -334,8 +332,7 @@ class CarInterface(object):
     can_sends = self.CC.update(enabled, self.CS, self.frame, \
                                c.actuators,
                                hud_v_cruise, c.hudControl.lanesVisible, \
-                               c.hudControl.leadVisible, \
+                               c.hudControl.leadVisible, c.hudControl.visualAlert)
-                               chime, chime_count, c.hudControl.visualAlert)
 
     self.frame += 1
     return can_sends

selfdrive/car/gm/values.py

@@ -1,8 +1,6 @@
 from cereal import car
 from selfdrive.car import dbc_dict
 
-AudibleAlert = car.CarControl.HUDControl.AudibleAlert
-
 class CAR:
   HOLDEN_ASTRA = "HOLDEN ASTRA RS-V BK 2017"
   VOLT = "CHEVROLET VOLT PREMIER 2017"
@@ -21,31 +19,12 @@ class CruiseButtons:
   MAIN        = 5
   CANCEL      = 6
 
-# Car chimes, beeps, blinker sounds etc
-class CM:
-  TOCK = 0x81
-  TICK = 0x82
-  LOW_BEEP = 0x84
-  HIGH_BEEP = 0x85
-  LOW_CHIME = 0x86
-  HIGH_CHIME = 0x87
-
 class AccState:
   OFF        = 0
   ACTIVE     = 1
   FAULTED    = 3
   STANDSTILL = 4
 
-AUDIO_HUD = {
-  AudibleAlert.none: (0, 0),
-  AudibleAlert.chimeEngage: (CM.HIGH_CHIME, 1),
-  AudibleAlert.chimeDisengage: (CM.HIGH_CHIME, 1),
-  AudibleAlert.chimeError: (CM.LOW_CHIME, 2),
-  AudibleAlert.chimePrompt: (CM.LOW_CHIME, 1),
-  AudibleAlert.chimeWarning1: (CM.LOW_CHIME, 2),
-  AudibleAlert.chimeWarning2: (CM.LOW_CHIME, -1),
-  AudibleAlert.chimeWarningRepeat: (CM.LOW_CHIME, -1)}
-
 def is_eps_status_ok(eps_status, car_fingerprint):
   valid_eps_status = []
   if car_fingerprint in SUPERCRUISE_CARS:

selfdrive/car/honda/carcontroller.py

@@ -70,7 +70,7 @@ def process_hud_alert(hud_alert):
 
 HUDData = namedtuple("HUDData",
                      ["pcm_accel", "v_cruise", "mini_car", "car", "X4",
-                      "lanes", "beep", "chime", "fcw", "acc_alert", "steer_required"])
+                      "lanes", "fcw", "acc_alert", "steer_required"])
 
 
 class CarController(object):
@@ -85,8 +85,7 @@ class CarController(object):
 
   def update(self, enabled, CS, frame, actuators, \
              pcm_speed, pcm_override, pcm_cancel_cmd, pcm_accel, \
-             hud_v_cruise, hud_show_lanes, hud_show_car, \
+             hud_v_cruise, hud_show_lanes, hud_show_car, hud_alert):
-             hud_alert, snd_beep, snd_chime):
 
     # *** apply brake hysteresis ***
     brake, self.braking, self.brake_steady = actuator_hystereses(actuators.brake, self.braking, self.brake_steady, CS.v_ego, CS.CP.carFingerprint)
@@ -113,15 +112,10 @@ class CarController(object):
     else:
       hud_car = 0
 
-    # For lateral control-only, send chimes as a beep since we don't send 0x1fa
-    if CS.CP.radarOffCan:
-      snd_beep = snd_beep if snd_beep != 0 else snd_chime
-
-    #print("{0} {1} {2}".format(chime, alert_id, hud_alert))
     fcw_display, steer_required, acc_alert = process_hud_alert(hud_alert)
 
     hud = HUDData(int(pcm_accel), int(round(hud_v_cruise)), 1, hud_car,
-                  0xc1, hud_lanes, int(snd_beep), snd_chime, fcw_display, acc_alert, steer_required)
+                  0xc1, hud_lanes, fcw_display, acc_alert, steer_required)
 
     # **** process the car messages ****
 
@@ -170,7 +164,7 @@ class CarController(object):
         ts = frame * DT_CTRL
         pump_on, self.last_pump_ts = brake_pump_hysteresis(apply_brake, self.apply_brake_last, self.last_pump_ts, ts)
         can_sends.append(hondacan.create_brake_command(self.packer, apply_brake, pump_on,
-          pcm_override, pcm_cancel_cmd, hud.chime, hud.fcw, idx, CS.CP.carFingerprint, CS.CP.isPandaBlack))
+          pcm_override, pcm_cancel_cmd, hud.fcw, idx, CS.CP.carFingerprint, CS.CP.isPandaBlack))
         self.apply_brake_last = apply_brake
 
         if CS.CP.enableGasInterceptor:

selfdrive/car/honda/carstate.py

@@ -38,6 +38,7 @@ def get_can_signals(CP):
       ("STEER_ANGLE", "STEERING_SENSORS", 0),
       ("STEER_ANGLE_RATE", "STEERING_SENSORS", 0),
       ("STEER_TORQUE_SENSOR", "STEER_STATUS", 0),
+      ("STEER_TORQUE_MOTOR", "STEER_STATUS", 0),
       ("LEFT_BLINKER", "SCM_FEEDBACK", 0),
       ("RIGHT_BLINKER", "SCM_FEEDBACK", 0),
       ("GEAR", "GEARBOX", 0),
@@ -93,6 +94,7 @@ def get_can_signals(CP):
       checks += [("BRAKE_MODULE", 50)]
     signals += [("CAR_GAS", "GAS_PEDAL_2", 0),
                 ("MAIN_ON", "SCM_FEEDBACK", 0),
+                ("CRUISE_CONTROL_LABEL", "ACC_HUD", 0),
                 ("EPB_STATE", "EPB_STATUS", 0),
                 ("CRUISE_SPEED", "ACC_HUD", 0)]
     checks += [("GAS_PEDAL_2", 100)]
@@ -187,6 +189,7 @@ class CarState(object):
     self.left_blinker_on = 0
     self.right_blinker_on = 0
 
+    self.cruise_mode = 0
     self.stopped = 0
 
     # vEgo kalman filter
@@ -298,11 +301,13 @@ class CarState(object):
       self.car_gas = cp.vl["GAS_PEDAL_2"]['CAR_GAS']
 
     self.steer_torque_driver = cp.vl["STEER_STATUS"]['STEER_TORQUE_SENSOR']
+    self.steer_torque_motor = cp.vl["STEER_STATUS"]['STEER_TORQUE_MOTOR']
     self.steer_override = abs(self.steer_torque_driver) > STEER_THRESHOLD[self.CP.carFingerprint]
 
     self.brake_switch = cp.vl["POWERTRAIN_DATA"]['BRAKE_SWITCH']
 
     if self.CP.radarOffCan:
+      self.cruise_mode = cp.vl["ACC_HUD"]['CRUISE_CONTROL_LABEL']
       self.stopped = cp.vl["ACC_HUD"]['CRUISE_SPEED'] == 252.
       self.cruise_speed_offset = calc_cruise_offset(0, self.v_ego)
       if self.CP.carFingerprint in (CAR.CIVIC_BOSCH, CAR.ACCORDH, CAR.CRV_HYBRID):

selfdrive/car/honda/hondacan.py

@@ -27,7 +27,7 @@ def get_lkas_cmd_bus(car_fingerprint, is_panda_black):
   return 2 if car_fingerprint in HONDA_BOSCH and not is_panda_black else 0
 
 
-def create_brake_command(packer, apply_brake, pump_on, pcm_override, pcm_cancel_cmd, chime, fcw, idx, car_fingerprint, is_panda_black):
+def create_brake_command(packer, apply_brake, pump_on, pcm_override, pcm_cancel_cmd, fcw, idx, car_fingerprint, is_panda_black):
   # TODO: do we loose pressure if we keep pump off for long?
   brakelights = apply_brake > 0
   brake_rq = apply_brake > 0
@@ -40,11 +40,14 @@ def create_brake_command(packer, apply_brake, pump_on, pcm_override, pcm_cancel_
     "CRUISE_FAULT_CMD": pcm_fault_cmd,
     "CRUISE_CANCEL_CMD": pcm_cancel_cmd,
     "COMPUTER_BRAKE_REQUEST": brake_rq,
-    "SET_ME_0X80": 0x80,
+    "SET_ME_1": 1,
     "BRAKE_LIGHTS": brakelights,
-    "CHIME": chime,
+    "CHIME": 0,
     # TODO: Why are there two bits for fcw? According to dbc file the first bit should also work
     "FCW": fcw << 1,
+    "AEB_REQ_1": 0,
+    "AEB_REQ_2": 0,
+    "AEB": 0,
   }
   bus = get_pt_bus(car_fingerprint, is_panda_black)
   return packer.make_can_msg("BRAKE_COMMAND", bus, values, idx)
@@ -83,7 +86,7 @@ def create_ui_commands(packer, pcm_speed, hud, car_fingerprint, is_metric, idx,
     'SET_ME_X48': 0x48,
     'STEERING_REQUIRED': hud.steer_required,
     'SOLID_LANES': hud.lanes,
-    'BEEP': hud.beep,
+    'BEEP': 0,
   }
   commands.append(packer.make_can_msg('LKAS_HUD', bus_lkas, lkas_hud_values, idx))
 

selfdrive/car/honda/interface.py

@@ -9,7 +9,7 @@ from selfdrive.config import Conversions as CV
 from selfdrive.controls.lib.drive_helpers import create_event, EventTypes as ET, get_events
 from selfdrive.controls.lib.vehicle_model import VehicleModel
 from selfdrive.car.honda.carstate import CarState, get_can_parser, get_cam_can_parser
-from selfdrive.car.honda.values import CruiseButtons, CAR, HONDA_BOSCH, AUDIO_HUD, VISUAL_HUD, CAMERA_MSGS
+from selfdrive.car.honda.values import CruiseButtons, CAR, HONDA_BOSCH, VISUAL_HUD, CAMERA_MSGS
 from selfdrive.car import STD_CARGO_KG, CivicParams, scale_rot_inertia, scale_tire_stiffness
 from selfdrive.controls.lib.planner import _A_CRUISE_MAX_V_FOLLOWING
 
@@ -171,7 +171,7 @@ class CarInterface(object):
       ret.steerRatio = 15.38  # 10.93 is end-to-end spec
       tire_stiffness_factor = 1.
       # Civic at comma has modified steering FW, so different tuning for the Neo in that car
-      is_fw_modified = os.getenv("DONGLE_ID") in ['99c94dc769b5d96e']
+      is_fw_modified = os.getenv("DONGLE_ID") in ['5b7c365c50084530']
       if is_fw_modified:
         ret.lateralTuning.pid.kf = 0.00004
 
@@ -405,12 +405,13 @@ class CarInterface(object):
     ret.gearShifter = self.CS.gear_shifter
 
     ret.steeringTorque = self.CS.steer_torque_driver
+    ret.steeringTorqueEps = self.CS.steer_torque_motor
     ret.steeringPressed = self.CS.steer_override
 
     # cruise state
     ret.cruiseState.enabled = self.CS.pcm_acc_status != 0
     ret.cruiseState.speed = self.CS.v_cruise_pcm * CV.KPH_TO_MS
-    ret.cruiseState.available = bool(self.CS.main_on)
+    ret.cruiseState.available = bool(self.CS.main_on) and not bool(self.CS.cruise_mode)
     ret.cruiseState.speedOffset = self.CS.cruise_speed_offset
     ret.cruiseState.standstill = False
 
@@ -487,7 +488,7 @@ class CarInterface(object):
       events.append(create_event('seatbeltNotLatched', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
     if self.CS.esp_disabled:
       events.append(create_event('espDisabled', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
-    if not self.CS.main_on:
+    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':
       events.append(create_event('reverseGear', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
@@ -564,7 +565,6 @@ class CarInterface(object):
       hud_v_cruise = 255
 
     hud_alert = VISUAL_HUD[c.hudControl.visualAlert.raw]
-    snd_beep, snd_chime = AUDIO_HUD[c.hudControl.audibleAlert.raw]
 
     pcm_accel = int(clip(c.cruiseControl.accelOverride, 0, 1) * 0xc6)
 
@@ -577,9 +577,7 @@ class CarInterface(object):
                                hud_v_cruise,
                                c.hudControl.lanesVisible,
                                hud_show_car=c.hudControl.leadVisible,
-                               hud_alert=hud_alert,
+                               hud_alert=hud_alert)
-                               snd_beep=snd_beep,
-                               snd_chime=snd_chime)
 
     self.frame += 1
     return can_sends

selfdrive/car/honda/values.py

@@ -1,7 +1,6 @@
 from cereal import car
 from selfdrive.car import dbc_dict
 
-AudibleAlert = car.CarControl.HUDControl.AudibleAlert
 VisualAlert = car.CarControl.HUDControl.VisualAlert
 
 # Car button codes
@@ -11,31 +10,6 @@ class CruiseButtons:
   CANCEL      = 2
   MAIN        = 1
 
-#car chimes: enumeration from dbc file. Chimes are for alerts and warnings
-class CM:
-  MUTE = 0
-  SINGLE = 3
-  DOUBLE = 4
-  REPEATED = 1
-  CONTINUOUS = 2
-
-#car beeps: enumeration from dbc file. Beeps are for engage and disengage
-class BP:
-  MUTE = 0
-  SINGLE = 3
-  TRIPLE = 2
-  REPEATED = 1
-
-AUDIO_HUD = {
-  AudibleAlert.none: (BP.MUTE, CM.MUTE),
-  AudibleAlert.chimeEngage: (BP.SINGLE, CM.MUTE),
-  AudibleAlert.chimeDisengage: (BP.SINGLE, CM.MUTE),
-  AudibleAlert.chimeError: (BP.MUTE, CM.DOUBLE),
-  AudibleAlert.chimePrompt: (BP.MUTE, CM.SINGLE),
-  AudibleAlert.chimeWarning1: (BP.MUTE, CM.DOUBLE),
-  AudibleAlert.chimeWarning2: (BP.MUTE, CM.REPEATED),
-  AudibleAlert.chimeWarningRepeat: (BP.MUTE, CM.REPEATED)}
-
 class AH:
   #[alert_idx, value]
   # See dbc files for info on values"

selfdrive/car/hyundai/interface.py

@@ -278,7 +278,7 @@ class CarInterface(object):
 
   def apply(self, c):
 
-    hud_alert = get_hud_alerts(c.hudControl.visualAlert, c.hudControl.audibleAlert)
+    hud_alert = get_hud_alerts(c.hudControl.visualAlert)
 
     can_sends = self.CC.update(c.enabled, self.CS, c.actuators,
                                c.cruiseControl.cancel, hud_alert)

selfdrive/car/hyundai/values.py

@@ -2,11 +2,10 @@ from cereal import car
 from selfdrive.car import dbc_dict
 
 VisualAlert = car.CarControl.HUDControl.VisualAlert
-AudibleAlert = car.CarControl.HUDControl.AudibleAlert
 
-def get_hud_alerts(visual_alert, audible_alert):
+def get_hud_alerts(visual_alert):
   if visual_alert == VisualAlert.steerRequired:
-    return 4 if audible_alert != AudibleAlert.none else 5
+    return 5
   else:
     return 0
 

selfdrive/car/toyota/carcontroller.py

@@ -10,7 +10,6 @@ from selfdrive.car.toyota.values import ECU, STATIC_MSGS, TSS2_CAR
 from selfdrive.can.packer import CANPacker
 
 VisualAlert = car.CarControl.HUDControl.VisualAlert
-AudibleAlert = car.CarControl.HUDControl.AudibleAlert
 
 # Accel limits
 ACCEL_HYST_GAP = 0.02  # don't change accel command for small oscilalitons within this value
@@ -53,25 +52,17 @@ def accel_hysteresis(accel, accel_steady, enabled):
   return accel, accel_steady
 
 
-def process_hud_alert(hud_alert, audible_alert):
+def process_hud_alert(hud_alert):
   # initialize to no alert
   steer = 0
   fcw = 0
-  sound1 = 0
-  sound2 = 0
 
   if hud_alert == VisualAlert.fcw:
     fcw = 1
   elif hud_alert == VisualAlert.steerRequired:
     steer = 1
 
-  if audible_alert == AudibleAlert.chimeWarningRepeat:
+  return steer, fcw
-    sound1 = 1
-  elif audible_alert != AudibleAlert.none:
-    # TODO: find a way to send single chimes
-    sound2 = 1
-
-  return steer, fcw, sound1, sound2
 
 
 def ipas_state_transition(steer_angle_enabled, enabled, ipas_active, ipas_reset_counter):
@@ -124,8 +115,8 @@ class CarController(object):
     self.packer = CANPacker(dbc_name)
 
   def update(self, enabled, CS, frame, actuators,
-             pcm_cancel_cmd, hud_alert, audible_alert, forwarding_camera,
+             pcm_cancel_cmd, hud_alert, forwarding_camera, left_line,
-             left_line, right_line, lead, left_lane_depart, right_lane_depart):
+             right_line, lead, left_lane_depart, right_lane_depart):
 
     # *** compute control surfaces ***
 
@@ -235,8 +226,8 @@ class CarController(object):
     # ui mesg is at 100Hz but we send asap if:
     # - there is something to display
     # - there is something to stop displaying
-    alert_out = process_hud_alert(hud_alert, audible_alert)
+    alert_out = process_hud_alert(hud_alert)
-    steer, fcw, sound1, sound2 = alert_out
+    steer, fcw = alert_out
 
     if (any(alert_out) and not self.alert_active) or \
        (not any(alert_out) and self.alert_active):
@@ -246,7 +237,7 @@ class CarController(object):
       send_ui = False
 
     if (frame % 100 == 0 or send_ui) and ECU.CAM in self.fake_ecus:
-      can_sends.append(create_ui_command(self.packer, steer, sound1, sound2, left_line, right_line, left_lane_depart, right_lane_depart))
+      can_sends.append(create_ui_command(self.packer, steer, 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/interface.py

@@ -55,7 +55,8 @@ class CarInterface(object):
     ret.enableCruise = not ret.enableGasInterceptor
 
     ret.steerActuatorDelay = 0.12  # Default delay, Prius has larger delay
-    if candidate != CAR.PRIUS:
+
+    if candidate not in [CAR.PRIUS, CAR.RAV4, CAR.RAV4H]: # These cars use LQR/INDI
       ret.lateralTuning.init('pid')
       ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
 
@@ -73,6 +74,18 @@ class CarInterface(object):
       ret.lateralTuning.indi.timeConstant = 1.0
       ret.lateralTuning.indi.actuatorEffectiveness = 1.0
 
+      # TODO: Determine if this is better than INDI
+      # ret.lateralTuning.init('lqr')
+      # ret.lateralTuning.lqr.scale = 1500.0
+      # ret.lateralTuning.lqr.ki = 0.01
+
+      # ret.lateralTuning.lqr.a = [0., 1., -0.22619643, 1.21822268]
+      # ret.lateralTuning.lqr.b = [-1.92006585e-04, 3.95603032e-05]
+      # ret.lateralTuning.lqr.c = [1., 0.]
+      # ret.lateralTuning.lqr.k = [-110.73572306, 451.22718255]
+      # ret.lateralTuning.lqr.l = [0.03233671, 0.03185757]
+      # ret.lateralTuning.lqr.dcGain = 0.002237852961363602
+
       ret.steerActuatorDelay = 0.5
 
     elif candidate in [CAR.RAV4, CAR.RAV4H]:
@@ -82,8 +95,17 @@ class CarInterface(object):
       ret.steerRatio = 16.88   # 14.5 is spec end-to-end
       tire_stiffness_factor = 0.5533
       ret.mass = 3650. * CV.LB_TO_KG + STD_CARGO_KG  # mean between normal and hybrid
-      ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.05]]
+      ret.lateralTuning.init('lqr')
-      ret.lateralTuning.pid.kf = 0.00006   # full torque for 10 deg at 80mph means 0.00007818594
+
+      ret.lateralTuning.lqr.scale = 1500.0
+      ret.lateralTuning.lqr.ki = 0.05
+
+      ret.lateralTuning.lqr.a = [0., 1., -0.22619643, 1.21822268]
+      ret.lateralTuning.lqr.b = [-1.92006585e-04, 3.95603032e-05]
+      ret.lateralTuning.lqr.c = [1., 0.]
+      ret.lateralTuning.lqr.k = [-110.73572306, 451.22718255]
+      ret.lateralTuning.lqr.l = [0.3233671, 0.3185757]
+      ret.lateralTuning.lqr.dcGain = 0.002237852961363602
 
     elif candidate == CAR.COROLLA:
       stop_and_go = False
@@ -293,6 +315,7 @@ class CarInterface(object):
     ret.steeringRate = self.CS.angle_steers_rate
 
     ret.steeringTorque = self.CS.steer_torque_driver
+    ret.steeringTorqueEps = self.CS.steer_torque_motor
     ret.steeringPressed = self.CS.steer_override
 
     # cruise state
@@ -389,8 +412,8 @@ class CarInterface(object):
 
     can_sends = self.CC.update(c.enabled, self.CS, self.frame,
                                c.actuators, c.cruiseControl.cancel, c.hudControl.visualAlert,
-                               c.hudControl.audibleAlert, self.forwarding_camera,
+                               self.forwarding_camera, c.hudControl.leftLaneVisible,
-                               c.hudControl.leftLaneVisible, c.hudControl.rightLaneVisible, c.hudControl.leadVisible,
+                               c.hudControl.rightLaneVisible, c.hudControl.leadVisible,
                                c.hudControl.leftLaneDepart, c.hudControl.rightLaneDepart)
 
     self.frame += 1

selfdrive/car/toyota/toyotacan.py

@@ -105,7 +105,7 @@ def create_fcw_command(packer, fcw):
   return packer.make_can_msg("ACC_HUD", 0, values)
 
 
-def create_ui_command(packer, steer, sound1, sound2, left_line, right_line, left_lane_depart, right_lane_depart):
+def create_ui_command(packer, steer, 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,
@@ -116,8 +116,8 @@ def create_ui_command(packer, steer, sound1, sound2, left_line, right_line, left
     "SET_ME_X02": 0x02,
     "SET_ME_X01": 1,
     "SET_ME_X01_2": 1,
-    "REPEATED_BEEPS": sound1,
+    "REPEATED_BEEPS": 0,
-    "TWO_BEEPS": sound2,
+    "TWO_BEEPS": 0,
     "LDA_ALERT": steer,
   }
   return packer.make_can_msg("LKAS_HUD", 0, values)

selfdrive/common/version.h

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

selfdrive/controls/controlsd.py

@@ -12,7 +12,7 @@ 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.controls.lib.model_parser import CAMERA_OFFSET
+from selfdrive.controls.lib.lane_planner import CAMERA_OFFSET
 from selfdrive.controls.lib.drive_helpers import get_events, \
                                                  create_event, \
                                                  EventTypes as ET, \
@@ -21,6 +21,7 @@ from selfdrive.controls.lib.drive_helpers import get_events, \
 from selfdrive.controls.lib.longcontrol import LongControl, STARTING_TARGET_SPEED
 from selfdrive.controls.lib.latcontrol_pid import LatControlPID
 from selfdrive.controls.lib.latcontrol_indi import LatControlINDI
+from selfdrive.controls.lib.latcontrol_lqr import LatControlLQR
 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
@@ -90,11 +91,16 @@ def data_sample(CI, CC, sm, can_sock, cal_status, cal_perc, overtemp, free_space
     cal_status = sm['liveCalibration'].calStatus
     cal_perc = sm['liveCalibration'].calPerc
 
+  cal_rpy = [0,0,0]
   if cal_status != Calibration.CALIBRATED:
     if cal_status == Calibration.UNCALIBRATED:
       events.append(create_event('calibrationIncomplete', [ET.NO_ENTRY, ET.SOFT_DISABLE, ET.PERMANENT]))
     else:
       events.append(create_event('calibrationInvalid', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
+  else:
+    rpy = sm['liveCalibration'].rpyCalib
+    if len(rpy) == 3:
+      cal_rpy = rpy
 
   # When the panda and controlsd do not agree on controls_allowed
   # we want to disengage openpilot. However the status from the panda goes through
@@ -112,7 +118,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)
+    driver_status.get_pose(sm['driverMonitoring'], params, cal_rpy)
 
   if driver_status.terminal_alert_cnt >= MAX_TERMINAL_ALERTS:
     events.append(create_event("tooDistracted", [ET.NO_ENTRY]))
@@ -255,8 +261,7 @@ def state_control(frame, rcv_frame, plan, path_plan, CS, CP, state, events, v_cr
   actuators.gas, actuators.brake = LoC.update(active, CS.vEgo, CS.brakePressed, CS.standstill, CS.cruiseState.standstill,
                                               v_cruise_kph, v_acc_sol, plan.vTargetFuture, a_acc_sol, CP)
   # Steering PID loop and lateral MPC
-  actuators.steer, actuators.steerAngle, lac_log = LaC.update(active, CS.vEgo, CS.steeringAngle, CS.steeringRate,
+  actuators.steer, actuators.steerAngle, lac_log = LaC.update(active, CS.vEgo, CS.steeringAngle, CS.steeringRate, CS.steeringTorqueEps, CS.steeringPressed, CP, VM, path_plan)
-                                                              CS.steeringPressed, CP, VM, path_plan)
 
   # Send a "steering required alert" if saturation count has reached the limit
   if LaC.sat_flag and CP.steerLimitAlert:
@@ -310,12 +315,11 @@ def data_send(sm, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, ca
   ldw_allowed = CS.vEgo > 12.5 and not blinker
 
   if len(list(sm['pathPlan'].rPoly)) == 4:
-    CC.hudControl.rightLaneDepart = bool(ldw_allowed and sm['pathPlan'].rPoly[3] > -(1 + CAMERA_OFFSET) and right_lane_visible)
+    CC.hudControl.rightLaneDepart = bool(ldw_allowed and sm['pathPlan'].rPoly[3] > -(1.08 + CAMERA_OFFSET) and right_lane_visible)
   if len(list(sm['pathPlan'].lPoly)) == 4:
-    CC.hudControl.leftLaneDepart = bool(ldw_allowed and sm['pathPlan'].lPoly[3] < (1 - CAMERA_OFFSET) and left_lane_visible)
+    CC.hudControl.leftLaneDepart = bool(ldw_allowed and sm['pathPlan'].lPoly[3] < (1.08 - CAMERA_OFFSET) and left_lane_visible)
 
   CC.hudControl.visualAlert = AM.visual_alert
-  CC.hudControl.audibleAlert = AM.audible_alert
 
   if not read_only:
     # send car controls over can
@@ -335,7 +339,7 @@ def data_send(sm, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, ca
     "alertStatus": AM.alert_status,
     "alertBlinkingRate": AM.alert_rate,
     "alertType": AM.alert_type,
-    "alertSound": "",  # no EON sounds yet
+    "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),
     "canMonoTimes": list(CS.canMonoTimes),
@@ -372,7 +376,9 @@ def data_send(sm, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, ca
 
   if CP.lateralTuning.which() == 'pid':
     dat.controlsState.lateralControlState.pidState = lac_log
-  else:
+  elif CP.lateralTuning.which() == 'lqr':
+    dat.controlsState.lateralControlState.lqrState = lac_log
+  elif CP.lateralTuning.which() == 'indi':
     dat.controlsState.lateralControlState.indiState = lac_log
   controlsstate.send(dat.to_bytes())
 
@@ -466,8 +472,10 @@ def controlsd_thread(gctx=None):
 
   if CP.lateralTuning.which() == 'pid':
     LaC = LatControlPID(CP)
-  else:
+  elif CP.lateralTuning.which() == 'indi':
     LaC = LatControlINDI(CP)
+  elif CP.lateralTuning.which() == 'lqr':
+    LaC = LatControlLQR(CP)
 
   driver_status = DriverStatus()
 
@@ -486,6 +494,9 @@ def controlsd_thread(gctx=None):
   sm['pathPlan'].sensorValid = True
   sm['pathPlan'].posenetValid = True
 
+  # detect sound card presence
+  sounds_available = not os.path.isfile('/EON') or (os.path.isdir('/proc/asound/card0') and open('/proc/asound/card0/state').read().strip() == 'ONLINE')
+
   # controlsd is driven by can recv, expected at 100Hz
   rk = Ratekeeper(100, print_delay_threshold=None)
 
@@ -518,6 +529,8 @@ def controlsd_thread(gctx=None):
       events.append(create_event('radarCanError', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
     if not CS.canValid:
       events.append(create_event('canError', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
+    if not sounds_available:
+      events.append(create_event('soundsUnavailable', [ET.NO_ENTRY, ET.PERMANENT]))
 
     # Only allow engagement with brake pressed when stopped behind another stopped car
     if CS.brakePressed and sm['plan'].vTargetFuture >= STARTING_TARGET_SPEED and not CP.radarOffCan and CS.vEgo < 0.3:

selfdrive/controls/lib/alertmanager.py

@@ -1,4 +1,4 @@
-from cereal import log
+from cereal import car, log
 from common.realtime import DT_CTRL
 from selfdrive.swaglog import cloudlog
 from selfdrive.controls.lib.alerts import ALERTS
@@ -7,7 +7,8 @@ import copy
 
 AlertSize = log.ControlsState.AlertSize
 AlertStatus = log.ControlsState.AlertStatus
-
+VisualAlert = car.CarControl.HUDControl.VisualAlert
+AudibleAlert = car.CarControl.HUDControl.AudibleAlert
 
 class AlertManager(object):
 
@@ -49,8 +50,8 @@ class AlertManager(object):
     self.alert_text_2 = ""
     self.alert_status = AlertStatus.normal
     self.alert_size = AlertSize.none
-    self.visual_alert = "none"
+    self.visual_alert = VisualAlert.none
-    self.audible_alert = "none"
+    self.audible_alert = AudibleAlert.none
     self.alert_rate = 0.
 
     if current_alert:

selfdrive/controls/lib/alerts.py

@@ -298,6 +298,13 @@ ALERTS = [
       AlertStatus.normal, AlertSize.mid,
       Priority.LOW, VisualAlert.none, AudibleAlert.chimeError, .4, 2., 3.),
 
+  Alert(
+      "soundsUnavailableNoEntry",
+      "openpilot Unavailable",
+      "Speaker not found",
+      AlertStatus.normal, AlertSize.mid,
+      Priority.LOW, VisualAlert.none, AudibleAlert.chimeError, .4, 2., 3.),
+
   Alert(
       "tooDistractedNoEntry",
       "openpilot Unavailable",
@@ -311,84 +318,84 @@ ALERTS = [
       "TAKE CONTROL IMMEDIATELY",
       "System Overheated",
       AlertStatus.critical, AlertSize.full,
-      Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarning2, .1, 2., 2.),
+      Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, .1, 2., 2.),
 
   Alert(
       "wrongGear",
       "TAKE CONTROL IMMEDIATELY",
       "Gear not D",
       AlertStatus.critical, AlertSize.full,
-      Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarning2, .1, 2., 2.),
+      Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, .1, 2., 2.),
 
   Alert(
       "calibrationInvalid",
       "TAKE CONTROL IMMEDIATELY",
       "Calibration Invalid: Reposition EON and Recalibrate",
       AlertStatus.critical, AlertSize.full,
-      Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarning2, .1, 2., 2.),
+      Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, .1, 2., 2.),
 
   Alert(
       "calibrationIncomplete",
       "TAKE CONTROL IMMEDIATELY",
       "Calibration in Progress",
       AlertStatus.critical, AlertSize.full,
-      Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarning2, .1, 2., 2.),
+      Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, .1, 2., 2.),
 
   Alert(
       "doorOpen",
       "TAKE CONTROL IMMEDIATELY",
       "Door Open",
       AlertStatus.critical, AlertSize.full,
-      Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarning2, .1, 2., 2.),
+      Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, .1, 2., 2.),
 
   Alert(
       "seatbeltNotLatched",
       "TAKE CONTROL IMMEDIATELY",
       "Seatbelt Unlatched",
       AlertStatus.critical, AlertSize.full,
-      Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarning2, .1, 2., 2.),
+      Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, .1, 2., 2.),
 
   Alert(
       "espDisabled",
       "TAKE CONTROL IMMEDIATELY",
       "ESP Off",
       AlertStatus.critical, AlertSize.full,
-      Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarning2, .1, 2., 2.),
+      Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, .1, 2., 2.),
 
   Alert(
       "lowBattery",
       "TAKE CONTROL IMMEDIATELY",
       "Low Battery",
       AlertStatus.critical, AlertSize.full,
-      Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarning2, .1, 2., 2.),
+      Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, .1, 2., 2.),
 
   Alert(
       "commIssue",
       "TAKE CONTROL IMMEDIATELY",
       "Communication Issue between Processes",
       AlertStatus.critical, AlertSize.full,
-      Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarning2, .1, 2., 2.),
+      Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, .1, 2., 2.),
 
   Alert(
       "radarCanError",
       "TAKE CONTROL IMMEDIATELY",
       "Radar Error: Restart the Car",
       AlertStatus.critical, AlertSize.full,
-      Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarning2, .1, 2., 2.),
+      Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, .1, 2., 2.),
 
   Alert(
       "radarFault",
       "TAKE CONTROL IMMEDIATELY",
       "Radar Error: Restart the Car",
       AlertStatus.critical, AlertSize.full,
-      Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarning2, .1, 2., 2.),
+      Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, .1, 2., 2.),
 
   Alert(
       "posenetInvalid",
       "TAKE CONTROL IMMEDIATELY",
       "Vision Failure: Check Camera View",
       AlertStatus.critical, AlertSize.full,
-      Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarning2, .1, 2., 2.),
+      Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, .1, 2., 2.),
 
   # Cancellation alerts causing immediate disabling
   Alert(
@@ -674,6 +681,13 @@ ALERTS = [
       AlertStatus.normal, AlertSize.mid,
       Priority.LOW_LOWEST, VisualAlert.none, AudibleAlert.none, 0., 0., .2),
 
+  Alert(
+      "soundsUnavailablePermanent",
+      "Speaker not found",
+      "Reboot your EON",
+      AlertStatus.normal, AlertSize.mid,
+      Priority.LOW_LOWEST, VisualAlert.none, AudibleAlert.none, 0., 0., .2),
+
   Alert(
       "vehicleModelInvalid",
       "Vehicle Parameter Identification Failed",

selfdrive/controls/lib/driver_monitor.py

@@ -3,31 +3,40 @@ from common.realtime import sec_since_boot, DT_CTRL, DT_DMON
 from selfdrive.controls.lib.drive_helpers import create_event, EventTypes as ET
 from common.filter_simple import FirstOrderFilter
 
-_AWARENESS_TIME = 180        # 3 minutes limit without user touching steering wheels make the car enter a terminal status
+_AWARENESS_TIME = 90.        # 1.5 minutes limit without user touching steering wheels make the car enter a terminal status
-_AWARENESS_PRE_TIME = 20.    # a first alert is issued 20s before expiration
+_AWARENESS_PRE_TIME_TILL_TERMINAL = 20.    # a first alert is issued 20s before expiration
-_AWARENESS_PROMPT_TIME = 5.  # a second alert is issued 5s before start decelerating the car
+_AWARENESS_PROMPT_TIME_TILL_TERMINAL = 5.  # a second alert is issued 5s before start decelerating the car
-_DISTRACTED_TIME = 7.
+_DISTRACTED_TIME = 10.
-_DISTRACTED_PRE_TIME = 4.
+_DISTRACTED_PRE_TIME_TILL_TERMINAL = 7.
-_DISTRACTED_PROMPT_TIME = 2.
+_DISTRACTED_PROMPT_TIME_TILL_TERMINAL = 5.
-# model output refers to center of cropped image, so need to apply the x displacement offset
+
 _FACE_THRESHOLD = 0.4
-_PITCH_WEIGHT = 1.5  # pitch matters a lot more
+_EYE_THRESHOLD = 0.4
+_BLINK_THRESHOLD = 0.2 # 0.225
+_PITCH_WEIGHT = 1.35 # 1.5  # pitch matters a lot more
 _METRIC_THRESHOLD = 0.4
-_PITCH_POS_ALLOWANCE = 0.08   # rad, to not be too sensitive on positive pitch
+_PITCH_POS_ALLOWANCE = 0.04 # 0.08  # rad, to not be too sensitive on positive pitch
-_PITCH_NATURAL_OFFSET = 0.1   # people don't seem to look straight when they drive relaxed, rather a bit up
+_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)
+_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)
-_STD_THRESHOLD = 0.1          # above this standard deviation consider the measurement invalid
 _DISTRACTED_FILTER_TS = 0.25  # 0.6Hz
 _VARIANCE_FILTER_TS = 20.     # 0.008Hz
 
 MAX_TERMINAL_ALERTS = 3      # not allowed to engage after 3 terminal alerts
+
+# model output refers to center of cropped image, so need to apply the x displacement offset
 RESIZED_FOCAL = 320.0
 H, W, FULL_W = 320, 160, 426
 
-def head_orientation_from_descriptor(angles_desc, pos_desc):
+class DistractedType(object):
+  NOT_DISTRACTED = 0
+  BAD_POSE = 1
+  BAD_BLINK = 2
+
+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 this should be calibrated
+
+  # TODO: calibrate based on position
   pitch_prnet = angles_desc[0]
   yaw_prnet = angles_desc[1]
   roll_prnet = angles_desc[2]
@@ -39,6 +48,11 @@ def head_orientation_from_descriptor(angles_desc, pos_desc):
   roll = roll_prnet
   pitch = pitch_prnet + pitch_focal_angle
   yaw = -yaw_prnet + yaw_focal_angle
+
+  # no calib for roll
+  pitch -= rpy_calib[1]
+  yaw -= rpy_calib[2]
+
   return np.array([roll, pitch, yaw])
 
 
@@ -50,15 +64,21 @@ class _DriverPose():
     self.yaw_offset = 0.
     self.pitch_offset = 0.
 
+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
@@ -70,52 +90,59 @@ class DriverStatus():
     self.ts_last_check = 0.
     self.face_detected = False
     self.terminal_alert_cnt = 0
-    self._set_timers()
+    self.step_change = 0.
+    self._set_timers(self.monitor_on)
 
   def _reset_filters(self):
     self.driver_distraction_filter.x = 0.
     self.variance_filter.x = 0.
     self.monitor_valid = True
 
-  def _set_timers(self):
+  def _set_timers(self, active_monitoring):
-    if self.monitor_on:
+    if active_monitoring:
-      self.threshold_pre = _DISTRACTED_PRE_TIME / _DISTRACTED_TIME
+      # when falling back from passive mode to active mode, reset awareness to avoid false alert
-      self.threshold_prompt = _DISTRACTED_PROMPT_TIME / _DISTRACTED_TIME
+      if self.step_change == DT_CTRL / _AWARENESS_TIME:
+        self.awareness = 1.
+      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
     else:
-      self.threshold_pre = _AWARENESS_PRE_TIME / _AWARENESS_TIME
+      self.threshold_pre = _AWARENESS_PRE_TIME_TILL_TERMINAL / _AWARENESS_TIME
-      self.threshold_prompt = _AWARENESS_PROMPT_TIME / _AWARENESS_TIME
+      self.threshold_prompt = _AWARENESS_PROMPT_TIME_TILL_TERMINAL / _AWARENESS_TIME
       self.step_change = DT_CTRL / _AWARENESS_TIME
 
-  def _is_driver_distracted(self, pose):
+  def _is_driver_distracted(self, pose, blink):
-    # to be tuned and to learn the driver's normal pose
+    # TODO: natural pose calib of each driver
     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.)
     pitch_error *= _PITCH_WEIGHT
-    metric = np.sqrt(yaw_error**2 + pitch_error**2)
+    pose_metric = np.sqrt(yaw_error**2 + pitch_error**2)
-    # TODO: do something with the eye states and combine them with head pose
+
-    return 1 if metric > _METRIC_THRESHOLD else 0
+    if pose_metric > _METRIC_THRESHOLD:
+      return DistractedType.BAD_POSE 
+    elif blink.left_blink>_BLINK_THRESHOLD and blink.right_blink>_BLINK_THRESHOLD:
+      return DistractedType.BAD_BLINK
+    else:
+      return DistractedType.NOT_DISTRACTED
 
 
-  def get_pose(self, driver_monitoring, params):
+  def get_pose(self, driver_monitoring, params, cal_rpy):
     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)
+    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.driver_distracted = self._is_driver_distracted(self.pose)
+    self.driver_distracted = self._is_driver_distracted(self.pose, self.blink)>0
     # first order filters
     self.driver_distraction_filter.update(self.driver_distracted)
-    self.variance_high = False #driver_monitoring.std > _STD_THRESHOLD
-
-    self.variance_filter.update(self.variance_high)
 
     monitor_param_on_prev = self.monitor_param_on
-    monitor_valid_prev = self.monitor_valid
 
     # don't check for param too often as it's a kernel call
     ts = sec_since_boot()
@@ -123,24 +150,26 @@ class DriverStatus():
       self.monitor_param_on = params.get("IsDriverMonitoringEnabled") == "1"
       self.ts_last_check = ts
 
-    self.monitor_valid = _monitor_hysteresis(self.variance_filter.x, monitor_valid_prev)
     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._set_timers(self.monitor_on and self.face_detected)
 
 
   def update(self, events, driver_engaged, ctrl_active, standstill):
+    if driver_engaged:
+      self.awareness = 1.
+      return events
 
     driver_engaged |= (self.driver_distraction_filter.x < 0.37 and self.monitor_on)
     awareness_prev = self.awareness
 
-    if (driver_engaged and self.awareness > 0.) or not ctrl_active:
+    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 = 1.
+      self.awareness = min(self.awareness + (2.75*(1.-self.awareness)+1.25)*self.step_change, 1.)
 
-    # only update if face is detected, driver is distracted and distraction filter is high
+    # should always be counting if distracted unless at standstill and reaching orange
-    if (not self.monitor_on or (self.driver_distraction_filter.x > 0.63 and self.driver_distracted and self.face_detected)) and \
+    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 \
        not (standstill and self.awareness - self.step_change <= self.threshold_prompt):
       self.awareness = max(self.awareness - self.step_change, -0.1)
 

selfdrive/controls/lib/lane_planner.py

@@ -0,0 +1,74 @@
+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 calc_d_poly(l_poly, r_poly, p_poly, l_prob, r_prob, lane_width):
+  # This will improve behaviour when lanes suddenly widen
+  lane_width = min(4.0, lane_width)
+  l_prob = l_prob * interp(abs(l_poly[3]), [2, 2.5], [1.0, 0.0])
+  r_prob = r_prob * interp(abs(r_poly[3]), [2, 2.5], [1.0, 0.0])
+
+  path_from_left_lane = l_poly.copy()
+  path_from_left_lane[3] -= lane_width / 2.0
+  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
+
+  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
+
+
+class LanePlanner(object):
+  def __init__(self):
+    self.l_poly = [0., 0., 0., 0.]
+    self.r_poly = [0., 0., 0., 0.]
+    self.p_poly = [0., 0., 0., 0.]
+    self.d_poly = [0., 0., 0., 0.]
+
+    self.lane_width_estimate = 3.7
+    self.lane_width_certainty = 1.0
+    self.lane_width = 3.7
+
+    self.l_prob = 0.
+    self.r_prob = 0.
+    self.lr_prob = 0.
+
+    self._path_pinv = compute_path_pinv()
+    self.x_points = np.arange(50)
+
+  def parse_model(self, md):
+    if len(md.leftLane.poly):
+      self.l_poly = np.array(md.leftLane.poly)
+      self.r_poly = np.array(md.rightLane.poly)
+      self.p_poly = np.array(md.path.poly)
+    else:
+      self.l_poly = model_polyfit(md.leftLane.points, self._path_pinv)  # left line
+      self.r_poly = model_polyfit(md.rightLane.points, self._path_pinv)  # right line
+      self.p_poly = model_polyfit(md.path.points, self._path_pinv)  # predicted path
+    self.l_prob = md.leftLane.prob  # left line prob
+    self.r_prob = md.rightLane.prob  # right line prob
+
+  def update_lane(self, v_ego):
+    # only offset left and right lane lines; offsetting p_poly does not make sense
+    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])
+    self.lane_width_estimate += 0.005 * (current_lane_width - self.lane_width_estimate)
+    speed_lane_width = interp(v_ego, [0., 31.], [2.8, 3.5])
+    self.lane_width = self.lane_width_certainty * self.lane_width_estimate + \
+                      (1 - self.lane_width_certainty) * speed_lane_width
+
+    self.d_poly = calc_d_poly(self.l_poly, self.r_poly, self.p_poly, self.l_prob, self.r_prob, self.lane_width)
+
+  def update(self, v_ego, md):
+    self.parse_model(md)
+    self.update_lane(v_ego)

selfdrive/controls/lib/latcontrol_helpers.py

@@ -60,30 +60,3 @@ def compute_path_pinv(l=50):
 
 def model_polyfit(points, path_pinv):
   return np.dot(path_pinv, [float(x) for x in points])
-
-
-def calc_desired_path(l_poly,
-                      r_poly,
-                      p_poly,
-                      l_prob,
-                      r_prob,
-                      p_prob,
-                      speed,
-                      lane_width=None):
-  # this function computes the poly for the center of the lane, averaging left and right polys
-  if lane_width is None:
-    lane_width = interp(speed, _LANE_WIDTH_BP, _LANE_WIDTH_V)
-
-  # lanes in US are ~3.6m wide
-  half_lane_poly = np.array([0., 0., 0., lane_width / 2.])
-  if l_prob + r_prob > 0.01:
-    c_poly = ((l_poly - half_lane_poly) * l_prob +
-              (r_poly + half_lane_poly) * r_prob) / (l_prob + r_prob)
-    c_prob = l_prob + r_prob - l_prob * r_prob
-  else:
-    c_poly = np.zeros(4)
-    c_prob = 0.
-
-  p_weight = 1.  # predicted path weight relatively to the center of the lane
-  d_poly = list((c_poly * c_prob + p_poly * p_prob * p_weight) / (c_prob + p_prob * p_weight))
-  return d_poly, c_poly, c_prob

selfdrive/controls/lib/latcontrol_indi.py

@@ -47,7 +47,7 @@ class LatControlINDI(object):
     self.output_steer = 0.
     self.counter = 0
 
-  def update(self, active, v_ego, angle_steers, angle_steers_rate, steer_override, CP, VM, path_plan):
+  def update(self, active, v_ego, angle_steers, angle_steers_rate, eps_torque, steer_override, CP, VM, path_plan):
     # Update Kalman filter
     y = np.matrix([[math.radians(angle_steers)], [math.radians(angle_steers_rate)]])
     self.x = np.dot(self.A_K, self.x) + np.dot(self.K, y)

selfdrive/controls/lib/latcontrol_lqr.py

@@ -0,0 +1,72 @@
+import numpy as np
+from selfdrive.controls.lib.drive_helpers import get_steer_max
+from common.numpy_fast import clip
+from cereal import log
+
+
+class LatControlLQR(object):
+  def __init__(self, CP, rate=100):
+    self.sat_flag = False
+    self.scale = CP.lateralTuning.lqr.scale
+    self.ki = CP.lateralTuning.lqr.ki
+
+
+    self.A = np.array(CP.lateralTuning.lqr.a).reshape((2,2))
+    self.B = np.array(CP.lateralTuning.lqr.b).reshape((2,1))
+    self.C = np.array(CP.lateralTuning.lqr.c).reshape((1,2))
+    self.K = np.array(CP.lateralTuning.lqr.k).reshape((1,2))
+    self.L = np.array(CP.lateralTuning.lqr.l).reshape((2,1))
+    self.dc_gain = CP.lateralTuning.lqr.dcGain
+
+    self.x_hat = np.array([[0], [0]])
+    self.i_unwind_rate = 0.3 / rate
+    self.i_rate = 1.0 / rate
+
+    self.reset()
+
+  def reset(self):
+    self.i_lqr = 0.0
+    self.output_steer = 0.0
+
+  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()
+
+    torque_scale = (0.45 + v_ego / 60.0)**2  # Scale actuator model with speed
+
+    # Subtract offset. Zero angle should correspond to zero torque
+    self.angle_steers_des = path_plan.angleSteers - path_plan.angleOffset
+    angle_steers -= path_plan.angleOffset
+
+    # Update Kalman filter
+    angle_steers_k = float(self.C.dot(self.x_hat))
+    e = angle_steers - angle_steers_k
+    self.x_hat = self.A.dot(self.x_hat) + self.B.dot(eps_torque / torque_scale) + self.L.dot(e)
+
+    if v_ego < 0.3 or not active:
+      lqr_log.active = False
+      self.reset()
+    else:
+      lqr_log.active = True
+
+      # LQR
+      u_lqr = float(self.angle_steers_des / self.dc_gain - self.K.dot(self.x_hat))
+
+      # 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)
+
+      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
+
+      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
+    return self.output_steer, float(self.angle_steers_des), lqr_log

selfdrive/controls/lib/latcontrol_pid.py

@@ -14,7 +14,7 @@ class LatControlPID(object):
   def reset(self):
     self.pid.reset()
 
-  def update(self, active, v_ego, angle_steers, angle_steers_rate, steer_override, CP, VM, path_plan):
+  def update(self, active, v_ego, angle_steers, angle_steers_rate, eps_torque, steer_override, CP, VM, path_plan):
     pid_log = log.ControlsState.LateralPIDState.new_message()
     pid_log.steerAngle = float(angle_steers)
     pid_log.steerRate = float(angle_steers_rate)

selfdrive/controls/lib/lateral_mpc/generator.cpp

@@ -23,8 +23,8 @@ int main( )
   OnlineData v_ref; // m/s
   OnlineData l_poly_r0, l_poly_r1, l_poly_r2, l_poly_r3;
   OnlineData r_poly_r0, r_poly_r1, r_poly_r2, r_poly_r3;
-  OnlineData p_poly_r0, p_poly_r1, p_poly_r2, p_poly_r3;
+  OnlineData d_poly_r0, d_poly_r1, d_poly_r2, d_poly_r3;
-  OnlineData l_prob, r_prob, p_prob;
+  OnlineData l_prob, r_prob;
   OnlineData lane_width;
 
   Control t;
@@ -39,26 +39,13 @@ int main( )
 
   auto poly_l = l_poly_r0*(xx*xx*xx) + l_poly_r1*(xx*xx) + l_poly_r2*xx + l_poly_r3;
   auto poly_r = r_poly_r0*(xx*xx*xx) + r_poly_r1*(xx*xx) + r_poly_r2*xx + r_poly_r3;
-  auto poly_p = p_poly_r0*(xx*xx*xx) + p_poly_r1*(xx*xx) + p_poly_r2*xx + p_poly_r3;
+  auto poly_d = d_poly_r0*(xx*xx*xx) + d_poly_r1*(xx*xx) + d_poly_r2*xx + d_poly_r3;
 
-  auto angle_l = atan(3*l_poly_r0*xx*xx + 2*l_poly_r1*xx + l_poly_r2);
+  auto angle_d = atan(3*d_poly_r0*xx*xx + 2*d_poly_r1*xx + d_poly_r2);
-  auto angle_r = atan(3*r_poly_r0*xx*xx + 2*r_poly_r1*xx + r_poly_r2);
-  auto angle_p = atan(3*p_poly_r0*xx*xx + 2*p_poly_r1*xx + p_poly_r2);
-
-  // given the lane width estimate, this is where we estimate the path given lane lines
-  auto path_from_left_lane = poly_l - lane_width/2.0;
-  auto path_from_right_lane = poly_r + lane_width/2.0;
-
-  // if the lanes are visible, drive in the center, otherwise follow the path
-  auto path = lr_prob * (l_prob * path_from_left_lane + r_prob * path_from_right_lane) / (l_prob + r_prob + 0.0001)
-    + (1-lr_prob) * poly_p;
-
-  auto angle = lr_prob * (l_prob * angle_l + r_prob * angle_r) / (l_prob + r_prob + 0.0001)
-    + (1-lr_prob) * angle_p;
 
   // When the lane is not visible, use an estimate of its position
-  auto weighted_left_lane = l_prob * poly_l + (1 - l_prob) * (path + lane_width/2.0);
+  auto weighted_left_lane = l_prob * poly_l + (1 - l_prob) * (poly_d + lane_width/2.0);
-  auto weighted_right_lane = r_prob * poly_r + (1 - r_prob) * (path - lane_width/2.0);
+  auto weighted_right_lane = r_prob * poly_r + (1 - r_prob) * (poly_d - lane_width/2.0);
 
   auto c_left_lane = exp(-(weighted_left_lane - yy));
   auto c_right_lane = exp(weighted_right_lane - yy);
@@ -67,12 +54,12 @@ int main( )
   Function h;
 
   // Distance errors
-  h << path - yy;
+  h << poly_d - yy;
   h << lr_prob * c_left_lane;
   h << lr_prob * c_right_lane;
 
   // Heading error
-  h << (v_ref + 1.0 ) * (angle - psi);
+  h << (v_ref + 1.0 ) * (angle_d - psi);
 
   // Angular rate error
   h << (v_ref + 1.0 ) * t;
@@ -88,12 +75,12 @@ int main( )
   Function hN;
 
   // Distance errors
-  hN << path - yy;
+  hN << poly_d - yy;
   hN << l_prob * c_left_lane;
   hN << r_prob * c_right_lane;
 
   // Heading errors
-  hN << (2.0 * v_ref + 1.0 ) * (angle - psi);
+  hN << (2.0 * v_ref + 1.0 ) * (angle_d - psi);
 
   BMatrix QN(4,4); QN.setAll(true);
   // QN(0,0) = 1.0;
@@ -125,7 +112,7 @@ int main( )
   ocp.subjectTo( deg2rad(-90) <= psi <= deg2rad(90));
   // more than absolute max steer angle
   ocp.subjectTo( deg2rad(-50) <= delta <= deg2rad(50));
-  ocp.setNOD(18);
+  ocp.setNOD(17);
 
   OCPexport mpc(ocp);
   mpc.set( HESSIAN_APPROXIMATION, GAUSS_NEWTON );

selfdrive/controls/lib/lateral_mpc/lateral_mpc.c

@@ -65,8 +65,8 @@ void init(double pathCost, double laneCost, double headingCost, double steerRate
 }
 
 int run_mpc(state_t * x0, log_t * solution,
-             double l_poly[4], double r_poly[4], double p_poly[4],
+             double l_poly[4], double r_poly[4], double d_poly[4],
-             double l_prob, double r_prob, double p_prob, double curvature_factor, double v_ref, double lane_width){
+             double l_prob, double r_prob, double curvature_factor, double v_ref, double lane_width){
 
   int    i;
 
@@ -84,16 +84,15 @@ int run_mpc(state_t * x0, log_t * solution,
     acadoVariables.od[i+8] = r_poly[2];
     acadoVariables.od[i+9] = r_poly[3];
 
-    acadoVariables.od[i+10] = p_poly[0];
+    acadoVariables.od[i+10] = d_poly[0];
-    acadoVariables.od[i+11] = p_poly[1];
+    acadoVariables.od[i+11] = d_poly[1];
-    acadoVariables.od[i+12] = p_poly[2];
+    acadoVariables.od[i+12] = d_poly[2];
-    acadoVariables.od[i+13] = p_poly[3];
+    acadoVariables.od[i+13] = d_poly[3];
 
 
     acadoVariables.od[i+14] = l_prob;
     acadoVariables.od[i+15] = r_prob;
-    acadoVariables.od[i+16] = p_prob;
+    acadoVariables.od[i+16] = lane_width;
-    acadoVariables.od[i+17] = lane_width;
 
   }
 

selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_common.h

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:f1d93e7b412f1573e2b6b22b11587bbed076d59a1c1c6d8f6d69eddc3998518c
+oid sha256:b175a66de26ad7bd788086a2d6a7ef6243eb2a0aac1ddcff39b00554a8960d97
 size 8823

selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_integrator.c

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:5cf12c96cffb69f1e659a20760c9ad3ab74ecce3d88aba1e50af359ab14c88da
+oid sha256:5848ec6e7975d6fee93187e0f41d6cba57cc0ebee6edf63ebddf3c7ad6f8f52c
-size 18662
+size 18622

selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_solver.c

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:269cf8ba0c80202e59352e7474d5aa768fa1ffc8268e051496d28629fa8cb144
+oid sha256:a2c030dd09379475b0247609d8a02f161f3e468e85480740d4abcf9c80868de0
-size 400285
+size 390405

selfdrive/controls/lib/lateral_mpc/libmpc_py.py

@@ -24,8 +24,8 @@ typedef struct {
 
 void init(double pathCost, double laneCost, double headingCost, double steerRateCost);
 int run_mpc(state_t * x0, log_t * solution,
-             double l_poly[4], double r_poly[4], double p_poly[4],
+             double l_poly[4], double r_poly[4], double d_poly[4],
-             double l_prob, double r_prob, double p_prob, double curvature_factor, double v_ref, double lane_width);
+             double l_prob, double r_prob, double curvature_factor, double v_ref, double lane_width);
 """)
 
 libmpc = ffi.dlopen(libmpc_fn)

selfdrive/controls/lib/model_parser.py

@@ -1,66 +0,0 @@
-from common.numpy_fast import interp
-import numpy as np
-from selfdrive.controls.lib.latcontrol_helpers import model_polyfit, calc_desired_path, compute_path_pinv
-
-CAMERA_OFFSET = 0.06  # m from center car to camera
-
-
-class ModelParser(object):
-  def __init__(self):
-    self.d_poly = [0., 0., 0., 0.]
-    self.c_poly = [0., 0., 0., 0.]
-    self.c_prob = 0.
-    self.last_model = 0.
-    self.lead_dist, self.lead_prob, self.lead_var = 0, 0, 1
-    self._path_pinv = compute_path_pinv()
-
-    self.lane_width_estimate = 3.7
-    self.lane_width_certainty = 1.0
-    self.lane_width = 3.7
-    self.l_prob = 0.
-    self.r_prob = 0.
-    self.x_points = np.arange(50)
-
-  def update(self, v_ego, md):
-    if len(md.leftLane.poly):
-      l_poly = np.array(md.leftLane.poly)
-      r_poly = np.array(md.rightLane.poly)
-      p_poly = np.array(md.path.poly)
-    else:
-      l_poly = model_polyfit(md.leftLane.points, self._path_pinv)  # left line
-      r_poly = model_polyfit(md.rightLane.points, self._path_pinv)  # right line
-      p_poly = model_polyfit(md.path.points, self._path_pinv)  # predicted path
-
-    # only offset left and right lane lines; offsetting p_poly does not make sense
-    l_poly[3] += CAMERA_OFFSET
-    r_poly[3] += CAMERA_OFFSET
-
-    p_prob = 1.  # model does not tell this probability yet, so set to 1 for now
-    l_prob = md.leftLane.prob  # left line prob
-    r_prob = md.rightLane.prob  # right line prob
-
-    # Find current lanewidth
-    lr_prob = l_prob * r_prob
-    self.lane_width_certainty += 0.05 * (lr_prob - self.lane_width_certainty)
-    current_lane_width = abs(l_poly[3] - r_poly[3])
-    self.lane_width_estimate += 0.005 * (current_lane_width - self.lane_width_estimate)
-    speed_lane_width = interp(v_ego, [0., 31.], [2.8, 3.5])
-    self.lane_width = self.lane_width_certainty * self.lane_width_estimate + \
-                      (1 - self.lane_width_certainty) * speed_lane_width
-
-    self.lead_dist = md.lead.dist
-    self.lead_prob = md.lead.prob
-    self.lead_var = md.lead.std**2
-
-    # compute target path
-    self.d_poly, self.c_poly, self.c_prob = calc_desired_path(
-      l_poly, r_poly, p_poly, l_prob, r_prob, p_prob, v_ego, self.lane_width)
-
-    self.r_poly = r_poly
-    self.r_prob = r_prob
-
-    self.l_poly = l_poly
-    self.l_prob = l_prob
-
-    self.p_poly = p_poly
-    self.p_prob = p_prob

selfdrive/controls/lib/pathplanner.py

@@ -2,12 +2,13 @@ import os
 import math
 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
-from selfdrive.controls.lib.model_parser import ModelParser
+from selfdrive.controls.lib.lane_planner import LanePlanner
 import selfdrive.messaging as messaging
 
 LOG_MPC = os.environ.get('LOG_MPC', False)
@@ -21,10 +22,7 @@ def calc_states_after_delay(states, v_ego, steer_angle, curvature_factor, steer_
 
 class PathPlanner(object):
   def __init__(self, CP):
-    self.MP = ModelParser()
+    self.LP = LanePlanner()
-
-    self.l_poly = [0., 0., 0., 0.]
-    self.r_poly = [0., 0., 0., 0.]
 
     self.last_cloudlog_t = 0
 
@@ -33,6 +31,7 @@ class PathPlanner(object):
 
     self.setup_mpc(CP.steerRateCost)
     self.solution_invalid_cnt = 0
+    self.path_offset_i = 0.0
 
   def setup_mpc(self, steer_rate_cost):
     self.libmpc = libmpc_py.libmpc
@@ -50,10 +49,6 @@ class PathPlanner(object):
     self.angle_steers_des_prev = 0.0
     self.angle_steers_des_time = 0.0
 
-    self.l_poly = libmpc_py.ffi.new("double[4]")
-    self.r_poly = libmpc_py.ffi.new("double[4]")
-    self.p_poly = libmpc_py.ffi.new("double[4]")
-
   def update(self, sm, CP, VM):
     v_ego = sm['carState'].vEgo
     angle_steers = sm['carState'].steeringAngle
@@ -62,23 +57,28 @@ class PathPlanner(object):
     angle_offset_average = sm['liveParameters'].angleOffsetAverage
     angle_offset_bias = sm['controlsState'].angleModelBias + angle_offset_average
 
-    self.MP.update(v_ego, sm['model'])
+    self.LP.update(v_ego, sm['model'])
 
     # Run MPC
     self.angle_steers_des_prev = self.angle_steers_des_mpc
     VM.update_params(sm['liveParameters'].stiffnessFactor, sm['liveParameters'].steerRatio)
     curvature_factor = VM.curvature_factor(v_ego)
-    self.l_poly = list(self.MP.l_poly)
+
-    self.r_poly = list(self.MP.r_poly)
+    # TODO: Check for active, override, and saturation
-    self.p_poly = list(self.MP.p_poly)
+    # if active:
+    #   self.path_offset_i += self.LP.d_poly[3] / (60.0 * 20.0)
+    #   self.path_offset_i = clip(self.path_offset_i, -0.5,  0.5)
+    #   self.LP.d_poly[3] += self.path_offset_i
+    # else:
+    #   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)
 
     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,
-                        self.l_poly, self.r_poly, self.p_poly,
+                        list(self.LP.l_poly), list(self.LP.r_poly), list(self.LP.d_poly),
-                        self.MP.l_prob, self.MP.r_prob, self.MP.p_prob, curvature_factor, v_ego_mpc, self.MP.lane_width)
+                        self.LP.l_prob, self.LP.r_prob, curvature_factor, v_ego_mpc, self.LP.lane_width)
 
     # reset to current steer angle if not active or overriding
     if active:
@@ -112,17 +112,16 @@ class PathPlanner(object):
     plan_send = messaging.new_message()
     plan_send.init('pathPlan')
     plan_send.valid = sm.all_alive_and_valid(service_list=['carState', 'controlsState', 'liveParameters', 'model'])
-    plan_send.pathPlan.laneWidth = float(self.MP.lane_width)
+    plan_send.pathPlan.laneWidth = float(self.LP.lane_width)
-    plan_send.pathPlan.dPoly = [float(x) for x in self.MP.d_poly]
+    plan_send.pathPlan.dPoly = [float(x) for x in self.LP.d_poly]
-    plan_send.pathPlan.cPoly = [float(x) for x in self.MP.c_poly]
+    plan_send.pathPlan.lPoly = [float(x) for x in self.LP.l_poly]
-    plan_send.pathPlan.cProb = float(self.MP.c_prob)
+    plan_send.pathPlan.lProb = float(self.LP.l_prob)
-    plan_send.pathPlan.lPoly = [float(x) for x in self.l_poly]
+    plan_send.pathPlan.rPoly = [float(x) for x in self.LP.r_poly]
-    plan_send.pathPlan.lProb = float(self.MP.l_prob)
+    plan_send.pathPlan.rProb = float(self.LP.r_prob)
-    plan_send.pathPlan.rPoly = [float(x) for x in self.r_poly]
+
-    plan_send.pathPlan.rProb = float(self.MP.r_prob)
     plan_send.pathPlan.angleSteers = float(self.angle_steers_des_mpc)
     plan_send.pathPlan.rateSteers = float(rate_desired)
-    plan_send.pathPlan.angleOffset = float(angle_offset_average)
+    plan_send.pathPlan.angleOffset = float(self.path_offset_i)
     plan_send.pathPlan.mpcSolutionValid = bool(plan_solution_valid)
     plan_send.pathPlan.paramsValid = bool(sm['liveParameters'].valid)
     plan_send.pathPlan.sensorValid = bool(sm['liveParameters'].sensorValid)

selfdrive/controls/lib/planner.py

@@ -3,7 +3,6 @@ import math
 import numpy as np
 from common.params import Params
 from common.numpy_fast import interp
-from common.kalman.simple_kalman import KF1D
 
 import selfdrive.messaging as messaging
 from cereal import car
@@ -95,9 +94,7 @@ class Planner(object):
     self.longitudinalPlanSource = 'cruise'
     self.fcw_checker = FCWChecker()
     self.fcw_enabled = fcw_enabled
-
+    self.path_x = np.arange(192)
-    self.model_v_kf = KF1D([[0.0],[0.0]], _MODEL_V_A, _MODEL_V_C, _MODEL_V_K)
-    self.model_v_kf_ready = False
 
     self.params = Params()
 
@@ -112,7 +109,6 @@ class Planner(object):
       slowest = min(solutions, key=solutions.get)
 
       self.longitudinalPlanSource = slowest
-
       # Choose lowest of MPC and cruise
       if slowest == 'mpc1':
         self.v_acc = self.mpc1.v_mpc
@@ -145,15 +141,21 @@ class Planner(object):
     enabled = (long_control_state == LongCtrlState.pid) or (long_control_state == LongCtrlState.stopping)
     following = lead_1.status and lead_1.dRel < 45.0 and lead_1.vLeadK > v_ego and lead_1.aLeadK > 0.0
 
-    if not self.model_v_kf_ready:
+    if len(sm['model'].path.poly):
-      self.model_v_kf.x = [[v_ego],[0.0]]
+      path = list(sm['model'].path.poly)
-      self.model_v_kf_ready = True
+
-
+      # Curvature of polynomial https://en.wikipedia.org/wiki/Curvature#Curvature_of_the_graph_of_a_function
-    if len(sm['model'].speed):
+      # y = a x^3 + b x^2 + c x + d, y' = 3 a x^2 + 2 b x + c, y'' = 6 a x + 2 b
-      self.model_v_kf.update(sm['model'].speed[SPEED_PERCENTILE_IDX])
+      # k = y'' / (1 + y'^2)^1.5
-
+      y_p = 3 * path[0] * self.path_x**2 + 2 * path[1] * self.path_x + path[2]
-    if self.params.get("LimitSetSpeedNeural") == "1":
+      y_pp = 6 * path[0] * self.path_x + 2 * path[1]
-      model_speed = self.model_v_kf.x[0][0]
+      curv = y_pp / (1. + y_p**2)**1.5
+
+      a_y_max = 2.975 - v_ego * 0.0375  # ~1.85 @ 75mph, ~2.6 @ 25mph
+      v_curvature = np.sqrt(a_y_max / np.clip(np.abs(curv), 1e-4, None))
+      model_speed = np.min(v_curvature)
+      # print(model_speed * CV.MS_TO_MPH, model_speed)
+      model_speed = max(20.0 * CV.MPH_TO_MS, model_speed) # Don't slow down below 20mph
     else:
       model_speed = MAX_SPEED
 
@@ -174,11 +176,9 @@ class Planner(object):
                                                     jerk_limits[1], jerk_limits[0],
                                                     LON_MPC_STEP)
 
-      # accel and jerk up limits are higher here to make model not limiting accel
-      # mainly done to prevent flickering of slowdown icon
       self.v_model, self.a_model = speed_smoother(self.v_acc_start, self.a_acc_start,
                                                     model_speed,
-                                                    2*accel_limits[1], 3*accel_limits[0],
+                                                    2*accel_limits[1], accel_limits[0],
                                                     2*jerk_limits[1], jerk_limits[0],
                                                     LON_MPC_STEP)
 
@@ -234,13 +234,13 @@ class Planner(object):
     plan_send.plan.radarStateMonoTime = sm.logMonoTime['radarState']
 
     # longitudal plan
-    plan_send.plan.vCruise = self.v_cruise
+    plan_send.plan.vCruise = float(self.v_cruise)
-    plan_send.plan.aCruise = self.a_cruise
+    plan_send.plan.aCruise = float(self.a_cruise)
-    plan_send.plan.vStart = self.v_acc_start
+    plan_send.plan.vStart = float(self.v_acc_start)
-    plan_send.plan.aStart = self.a_acc_start
+    plan_send.plan.aStart = float(self.a_acc_start)
-    plan_send.plan.vTarget = self.v_acc
+    plan_send.plan.vTarget = float(self.v_acc)
-    plan_send.plan.aTarget = self.a_acc
+    plan_send.plan.aTarget = float(self.a_acc)
-    plan_send.plan.vTargetFuture = self.v_acc_future
+    plan_send.plan.vTargetFuture = float(self.v_acc_future)
     plan_send.plan.hasLead = self.mpc1.prev_lead_status
     plan_send.plan.longitudinalPlanSource = self.longitudinalPlanSource
 

selfdrive/controls/lib/radar_helpers.py

@@ -25,14 +25,9 @@ _VLEAD_K = [[0.1988689], [0.28555364]]
 class Track(object):
   def __init__(self):
     self.ekf = None
-    self.initted = False
+    self.cnt = 0
 
   def update(self, d_rel, y_rel, v_rel, v_ego_t_aligned, measured):
-    if self.initted:
-      # pylint: disable=access-member-before-definition
-      self.vLeadPrev = self.vLead
-      self.vRelPrev = self.vRel
-
     # relative values, copy
     self.dRel = d_rel   # LONG_DIST
     self.yRel = y_rel   # -LAT_DIST
@@ -42,17 +37,12 @@ class Track(object):
     # computed velocity and accelerations
     self.vLead = self.vRel + v_ego_t_aligned
 
-    if not self.initted:
+    if self.cnt == 0:
-      self.initted = True
-      self.aLeadTau = _LEAD_ACCEL_TAU
-      self.cnt = 1
-      self.vision_cnt = 0
-      self.vision = False
       self.kf = KF1D([[self.vLead], [0.0]], _VLEAD_A, _VLEAD_C, _VLEAD_K)
     else:
       self.kf.update(self.vLead)
 
-      self.cnt += 1
+    self.cnt += 1
 
     self.vLeadK = float(self.kf.x[SPEED][0])
     self.aLeadK = float(self.kf.x[ACCEL][0])
@@ -67,6 +57,10 @@ class Track(object):
     # Weigh y higher since radar is inaccurate in this dimension
     return [self.dRel, self.yRel*2, self.vRel]
 
+  def reset_a_lead(self, aLeadK, aLeadTau):
+    self.kf = KF1D([[self.vLead], [aLeadK]], _VLEAD_A, _VLEAD_C, _VLEAD_K)
+    self.aLeadK = aLeadK
+    self.aLeadTau = aLeadTau
 
 def mean(l):
   return sum(l) / len(l)
@@ -115,11 +109,17 @@ class Cluster(object):
 
   @property
   def aLeadK(self):
-    return mean([t.aLeadK for t in self.tracks])
+    if all(t.cnt <= 1 for t in self.tracks):
+      return 0.
+    else:
+      return mean([t.aLeadK for t in self.tracks if t.cnt > 1])
 
   @property
   def aLeadTau(self):
-    return mean([t.aLeadTau for t in self.tracks])
+    if all(t.cnt <= 1 for t in self.tracks):
+      return _LEAD_ACCEL_TAU
+    else:
+      return mean([t.aLeadTau for t in self.tracks if t.cnt > 1])
 
   @property
   def measured(self):

selfdrive/controls/radard.py

@@ -143,15 +143,24 @@ class RadarD(object):
           clusters[cluster_i] = Cluster()
         clusters[cluster_i].add(self.tracks[idens[idx]])
     elif len(track_pts) == 1:
+      # FIXME: cluster_point_centroid hangs forever if len(track_pts) == 1
+      cluster_idxs = [0]
       clusters = [Cluster()]
       clusters[0].add(self.tracks[idens[0]])
     else:
       clusters = []
 
+    # if a new point, reset accel to the rest of the cluster
+    for idx in xrange(len(track_pts)):
+      if self.tracks[idens[idx]].cnt <= 1:
+        aLeadK = clusters[cluster_idxs[idx]].aLeadK
+        aLeadTau = clusters[cluster_idxs[idx]].aLeadTau
+        self.tracks[idens[idx]].reset_a_lead(aLeadK, aLeadTau)
+
     # *** publish radarState ***
     dat = messaging.new_message()
     dat.init('radarState')
-    dat.valid = sm.all_alive_and_valid(service_list=['controlsState'])
+    dat.valid = sm.all_alive_and_valid(service_list=['controlsState', 'model'])
     dat.radarState.mdMonoTime = self.last_md_ts
     dat.radarState.canMonoTimes = list(rr.canMonoTimes)
     dat.radarState.radarErrors = list(rr.errors)

selfdrive/controls/tests/test_lateral_mpc.py

@@ -1,9 +1,9 @@
 import unittest
-import copy
 import numpy as np
 from selfdrive.car.honda.interface import CarInterface
 from selfdrive.controls.lib.lateral_mpc import libmpc_py
 from selfdrive.controls.lib.vehicle_model import VehicleModel
+from selfdrive.controls.lib.lane_planner import calc_d_poly
 
 
 def run_mpc(v_ref=30., x_init=0., y_init=0., psi_init=0., delta_init=0.,
@@ -16,13 +16,17 @@ def run_mpc(v_ref=30., x_init=0., y_init=0., psi_init=0., delta_init=0.,
 
   mpc_solution = libmpc_py.ffi.new("log_t *")
 
-  p_l = copy.copy(poly_l)
+  p_l = poly_l.copy()
   p_l[3] += poly_shift
-  p_r = copy.copy(poly_r)
+
+  p_r = poly_r.copy()
   p_r[3] += poly_shift
-  p_p = copy.copy(poly_p)
+
+  p_p = poly_p.copy()
   p_p[3] += poly_shift
 
+  d_poly = calc_d_poly(p_l, p_r, p_p, l_prob, r_prob, lane_width)
+
   CP = CarInterface.get_params("HONDA CIVIC 2016 TOURING", {})
   VM = VehicleModel(CP)
 
@@ -31,7 +35,7 @@ def run_mpc(v_ref=30., x_init=0., y_init=0., psi_init=0., delta_init=0.,
 
   l_poly = libmpc_py.ffi.new("double[4]", map(float, p_l))
   r_poly = libmpc_py.ffi.new("double[4]", map(float, p_r))
-  p_poly = libmpc_py.ffi.new("double[4]", map(float, p_p))
+  d_poly = libmpc_py.ffi.new("double[4]", map(float, d_poly))
 
   cur_state = libmpc_py.ffi.new("state_t *")
   cur_state[0].x = x_init
@@ -41,7 +45,7 @@ def run_mpc(v_ref=30., x_init=0., y_init=0., psi_init=0., delta_init=0.,
 
   # converge in no more than 20 iterations
   for _ in range(20):
-    libmpc.run_mpc(cur_state, mpc_solution, l_poly, r_poly, p_poly, l_prob, r_prob, p_prob,
+    libmpc.run_mpc(cur_state, mpc_solution, l_poly, r_poly, d_poly, l_prob, r_prob,
                    curvature_factor, v_ref, lane_width)
 
   return mpc_solution
@@ -119,3 +123,7 @@ class TestLateralMpc(unittest.TestCase):
     sol = run_mpc(y_init=y_init)
     for y in list(sol[0].y):
       self.assertGreaterEqual(y_init, abs(y))
+
+
+if __name__ == "__main__":
+  unittest.main()

selfdrive/locationd/calibrationd.py

@@ -8,7 +8,7 @@ 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, get_camera_frame_from_model_frame, get_camera_frame_from_medmodel_frame
+from common.transformations.model import model_height
 from common.transformations.camera import view_frame_from_device_frame, get_view_frame_from_road_frame, \
                                           eon_intrinsics, get_calib_from_vp, H, W
 
@@ -78,21 +78,19 @@ class Calibrator(object):
                       "valid_points": len(self.vps)}
         self.params.put("CalibrationParams", json.dumps(cal_params))
       return new_vp
+    else:
+      return None
 
   def send_data(self, livecalibration):
     calib = get_calib_from_vp(self.vp)
     extrinsic_matrix = get_view_frame_from_road_frame(0, calib[1], calib[2], model_height)
-    ke = eon_intrinsics.dot(extrinsic_matrix)
-    warp_matrix = get_camera_frame_from_model_frame(ke)
-    warp_matrix_big = get_camera_frame_from_medmodel_frame(ke)
 
     cal_send = messaging.new_message()
     cal_send.init('liveCalibration')
     cal_send.liveCalibration.calStatus = self.cal_status
     cal_send.liveCalibration.calPerc = min(len(self.vps) * 100 // INPUTS_NEEDED, 100)
-    cal_send.liveCalibration.warpMatrix2 = [float(x) for x in warp_matrix.flatten()]
-    cal_send.liveCalibration.warpMatrixBig = [float(x) for x in warp_matrix_big.flatten()]
     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())
 

selfdrive/locationd/locationd_yawrate.cc

@@ -12,9 +12,11 @@
 
 void Localizer::update_state(const Eigen::Matrix<double, 1, 2> &C, const double R, double current_time, double meas) {
   double dt = current_time - prev_update_time;
-  prev_update_time = current_time;
+
-  if (dt < 1.0e-9) {
+  if (dt < 0) {
-    return;
+    dt = 0;
+  } else {
+    prev_update_time = current_time;
   }
 
   // x = A * x;
@@ -73,6 +75,7 @@ Localizer::Localizer() {
 void Localizer::handle_log(cereal::Event::Reader event) {
   double current_time = event.getLogMonoTime() / 1.0e9;
 
+  // Initialize update_time on first update
   if (prev_update_time < 0) {
     prev_update_time = current_time;
   }
@@ -117,6 +120,16 @@ extern "C" {
     Localizer * loc = (Localizer*) localizer;
     return loc->x[1];
   }
+
+  void localizer_set_yaw(void * localizer, double yaw) {
+    Localizer * loc = (Localizer*) localizer;
+    loc->x[0] = yaw;
+  }
+  void localizer_set_bias(void * localizer, double bias) {
+    Localizer * loc = (Localizer*) localizer;
+    loc->x[1] = bias;
+  }
+
   double localizer_get_t(void * localizer) {
     Localizer * loc = (Localizer*) localizer;
     return loc->prev_update_time;

selfdrive/locationd/paramsd.cc

@@ -31,9 +31,9 @@ int main(int argc, char *argv[]) {
   zmq_pollitem_t polls[num_polls] = {{0}};
   polls[0].socket = controls_state_sock;
   polls[0].events = ZMQ_POLLIN;
-  polls[1].socket = sensor_events_sock;
+  polls[1].socket = camera_odometry_sock;
   polls[1].events = ZMQ_POLLIN;
-  polls[2].socket = camera_odometry_sock;
+  polls[2].socket = sensor_events_sock;
   polls[2].events = ZMQ_POLLIN;
 
   // Read car params
@@ -122,8 +122,9 @@ int main(int argc, char *argv[]) {
         localizer.handle_log(event);
 
         auto which = event.which();
+        // Throw vision failure if posenet and odometric speed too different
         if (which == cereal::Event::CAMERA_ODOMETRY){
-          if (std::abs(localizer.posenet_speed - localizer.car_speed) > std::max(0.5 * localizer.car_speed, 5.0)) {
+          if (std::abs(localizer.posenet_speed - localizer.car_speed) > std::max(0.4 * localizer.car_speed, 5.0)) {
               posenet_invalid_count++;
             } else {
             posenet_invalid_count = 0;
@@ -155,7 +156,7 @@ int main(int argc, char *argv[]) {
             live_params.setStiffnessFactor(learner.x);
             live_params.setSteerRatio(learner.sR);
             live_params.setPosenetSpeed(localizer.posenet_speed);
-            live_params.setPosenetValid(posenet_invalid_count < 5);
+            live_params.setPosenetValid(posenet_invalid_count < 4);
 
             auto words = capnp::messageToFlatArray(msg);
             auto bytes = words.asBytes();

selfdrive/loggerd/uploader.py

@@ -17,7 +17,7 @@ from selfdrive.swaglog import cloudlog
 from selfdrive.loggerd.config import ROOT
 
 from common.params import Params
-from common.api import api_get
+from common.api import Api
 
 fake_upload = os.getenv("FAKEUPLOAD") is not None
 
@@ -93,9 +93,9 @@ def is_on_hotspot():
     return False
 
 class Uploader(object):
-  def __init__(self, dongle_id, access_token, root):
+  def __init__(self, dongle_id, private_key, root):
     self.dongle_id = dongle_id
-    self.access_token = access_token
+    self.api = Api(dongle_id, private_key)
     self.root = root
 
     self.upload_thread = None
@@ -168,11 +168,11 @@ class Uploader(object):
 
   def do_upload(self, key, fn):
     try:
-      url_resp = api_get("v1.2/"+self.dongle_id+"/upload_url/", timeout=10, path=key, access_token=self.access_token)
+      url_resp = self.api.get("v1.3/"+self.dongle_id+"/upload_url/", timeout=10, path=key, access_token=self.api.get_token())
       url_resp_json = json.loads(url_resp.text)
       url = url_resp_json['url']
       headers = url_resp_json['headers']
-      cloudlog.info("upload_url v1.2 %s %s", url, str(headers))
+      cloudlog.info("upload_url v1.3 %s %s", url, str(headers))
 
       if fake_upload:
         cloudlog.info("*** WARNING, THIS IS A FAKE UPLOAD TO %s ***" % url)
@@ -240,13 +240,14 @@ def uploader_fn(exit_event):
   cloudlog.info("uploader_fn")
 
   params = Params()
-  dongle_id, access_token = params.get("DongleId"), params.get("AccessToken")
+  dongle_id = params.get("DongleId")
+  private_key = open("/persist/comma/id_rsa").read()
 
-  if dongle_id is None or access_token is None:
+  if dongle_id is None or private_key is None:
-    cloudlog.info("uploader MISSING DONGLE_ID or ACCESS_TOKEN")
+    cloudlog.info("uploader missing dongle_id or private_key")
-    raise Exception("uploader can't start without dongle id and access token")
+    raise Exception("uploader can't start without dongle id and private key")
 
-  uploader = Uploader(dongle_id, access_token, ROOT)
+  uploader = Uploader(dongle_id, private_key, 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 != 3)
+    neos_update_required = version < 10 or (version == 10 and revision != 4)
 
   if neos_update_required:
     # update continue.sh before updating NEOS
@@ -518,6 +518,9 @@ def main():
   # the flippening!
   os.system('LD_LIBRARY_PATH="" content insert --uri content://settings/system --bind name:s:user_rotation --bind value:i:1')
 
+  # disable bluetooth
+  os.system('service call bluetooth_manager 8')
+
   if os.getenv("NOLOG") is not None:
     del managed_processes['loggerd']
     del managed_processes['tombstoned']

selfdrive/registration.py

@@ -71,6 +71,10 @@ def register():
     os.rename("/persist/comma/id_rsa.tmp", "/persist/comma/id_rsa")
     os.rename("/persist/comma/id_rsa.tmp.pub", "/persist/comma/id_rsa.pub")
 
+  # make key readable by app users (ai.comma.plus.offroad)
+  os.chmod('/persist/comma/', 0o755)
+  os.chmod('/persist/comma/id_rsa', 0o744)
+
   dongle_id, access_token = params.get("DongleId"), params.get("AccessToken")
   public_key = open("/persist/comma/id_rsa.pub").read()
 

selfdrive/test/openpilotci_upload.py

@@ -0,0 +1,22 @@
+#!/usr/bin/env python2
+import os
+import sys
+import subprocess
+from azure.storage.blob import BlockBlobService
+
+def upload_file(path, name):
+  sas_token = os.getenv("TOKEN", None)
+  if sas_token is not None:
+    service = BlockBlobService(account_name="commadataci", sas_token=sas_token)
+  else:
+    account_key = subprocess.check_output("az storage account keys list --account-name commadataci --output tsv --query '[0].value'", shell=True)
+    service = BlockBlobService(account_name="commadataci", account_key=account_key)
+  service.create_blob_from_path("openpilotci", name, path)
+  return "https://commadataci.blob.core.windows.net/openpilotci/" + name
+
+if __name__ == "__main__":
+  for f in sys.argv[1:]:
+    name = os.path.basename(f)
+    url = upload_file(f, name)
+    print url
+

selfdrive/test/plant/plant.py

@@ -1,6 +1,7 @@
 #!/usr/bin/env python
 import os
 import struct
+import time
 from collections import namedtuple
 import numpy as np
 
@@ -133,6 +134,11 @@ class Plant(object):
     self.ts = 1./rate
 
     self.cp = get_car_can_parser()
+    self.response_seen = False
+
+    time.sleep(1)
+    messaging.drain_sock(Plant.sendcan)
+    messaging.drain_sock(Plant.controls_state)
 
   def close(self):
     Plant.logcan.close()
@@ -158,13 +164,18 @@ class Plant(object):
 
     # ******** get messages sent to the car ********
     can_msgs = []
-    for a in messaging.drain_sock(Plant.sendcan):
+    for a in messaging.drain_sock(Plant.sendcan, wait_for_one=self.response_seen):
       can_msgs.extend(can_capnp_to_can_list(a.sendcan, [0,2]))
+
+    # After the first response the car is done fingerprinting, so we can run in lockstep with controlsd
+    if can_msgs:
+      self.response_seen = True
+
     self.cp.update_can(can_msgs)
 
     # ******** get controlsState messages for plotting ***
     controls_state_msgs = []
-    for a in messaging.drain_sock(Plant.controls_state):
+    for a in messaging.drain_sock(Plant.controls_state, wait_for_one=self.response_seen):
       controls_state_msgs.append(a.controlsState)
 
     fcw = None
@@ -217,7 +228,7 @@ class Plant(object):
     vls_tuple = namedtuple('vls', [
            'XMISSION_SPEED',
            'WHEEL_SPEED_FL', 'WHEEL_SPEED_FR', 'WHEEL_SPEED_RL', 'WHEEL_SPEED_RR',
-           'STEER_ANGLE', 'STEER_ANGLE_RATE', 'STEER_TORQUE_SENSOR',
+           'STEER_ANGLE', 'STEER_ANGLE_RATE', 'STEER_TORQUE_SENSOR', 'STEER_TORQUE_MOTOR',
            'LEFT_BLINKER', 'RIGHT_BLINKER',
            'GEAR',
            'WHEELS_MOVING',
@@ -250,7 +261,7 @@ class Plant(object):
     vls = vls_tuple(
            self.speed_sensor(speed),
            self.speed_sensor(speed), self.speed_sensor(speed), self.speed_sensor(speed), self.speed_sensor(speed),
-           self.angle_steer, self.angle_steer_rate, 0, #Steer torque sensor
+           self.angle_steer, self.angle_steer_rate, 0, 0,#Steer torque sensor
            0, 0,  # Blinkers
            self.gear_choice,
            speed != 0,
@@ -325,7 +336,6 @@ class Plant(object):
     msg_data = fix(msg_data, 0xe4)
     can_msgs.append([0xe4, 0, msg_data, 2])
 
-    Plant.logcan.send(can_list_to_can_capnp(can_msgs))
 
     # Fake sockets that controlsd subscribes to
     live_parameters = messaging.new_message()
@@ -388,10 +398,13 @@ class Plant(object):
 
       cal.liveCalibration.calStatus = 1
       cal.liveCalibration.calPerc = 100
+      cal.liveCalibration.rpyCalib = [0.] * 3
       # fake values?
       Plant.model.send(md.to_bytes())
       Plant.cal.send(cal.to_bytes())
 
+    Plant.logcan.send(can_list_to_can_capnp(can_msgs))
+
     # ******** update prevs ********
     self.speed = speed
     self.distance = distance
@@ -401,7 +414,11 @@ class Plant(object):
     self.distance_prev = distance
     self.distance_lead_prev = distance_lead
 
-    self.rk.keep_time()
+    if self.response_seen:
+      self.rk.monitor_time()
+    else:
+      self.rk.keep_time()
+
     return {
       "distance": distance,
       "speed": speed,

selfdrive/test/test_car_models_openpilot.py

@@ -0,0 +1,494 @@
+#!/usr/bin/env python2
+import shutil
+import time
+import zmq
+import os
+import sys
+import signal
+import subprocess
+import requests
+from cereal import car
+
+import selfdrive.manager as manager
+from selfdrive.services import service_list
+import selfdrive.messaging as messaging
+from common.params import Params
+from common.basedir import BASEDIR
+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
+from selfdrive.car.ford.values import CAR as FORD
+from selfdrive.car.hyundai.values import CAR as HYUNDAI
+from selfdrive.car.chrysler.values import CAR as CHRYSLER
+from selfdrive.car.subaru.values import CAR as SUBARU
+from selfdrive.car.mock.values import CAR as MOCK
+
+
+os.environ['NOCRASH'] = '1'
+
+
+def wait_for_socket(name, timeout=10.0):
+  socket = messaging.sub_sock(service_list[name].port)
+  cur_time = time.time()
+
+  r = None
+  while time.time() - cur_time < timeout:
+    print("waiting for %s" % name)
+    try:
+      r = socket.recv(zmq.NOBLOCK)
+      break
+    except zmq.error.Again:
+      pass
+    time.sleep(0.5)
+  return r
+
+def get_route_logs(route_name):
+  for log_f in ["rlog.bz2", "fcamera.hevc"]:
+    log_path = os.path.join("/tmp", "%s--0--%s" % (route_name.replace("|", "_"), log_f))
+
+    if not os.path.isfile(log_path):
+      log_url = "https://commadataci.blob.core.windows.net/openpilotci/%s/0/%s" % (route_name.replace("|", "/"), log_f)
+      r = requests.get(log_url)
+
+      if r.status_code == 200:
+        with open(log_path, "w") as f:
+          f.write(r.content)
+      else:
+        sys.exit(-1)
+
+routes = {
+
+  "975b26878285314d|2018-12-25--14-42-13": {
+    'carFingerprint': CHRYSLER.PACIFICA_2018_HYBRID,
+    'enableCamera': True,
+  },
+  "b0c9d2329ad1606b|2019-01-06--10-11-23": {
+    'carFingerprint': CHRYSLER.PACIFICA_2017_HYBRID,
+    'enableCamera': True,
+  },
+  "0607d2516fc2148f|2019-02-13--23-03-16": {
+    'carFingerprint': CHRYSLER.PACIFICA_2019_HYBRID,
+    'enableCamera': True,
+  },
+  # This pacifica was removed because the fingerprint seemed from a Volt
+  #"9f7a7e50a51fb9db|2019-01-03--14-05-01": {
+  #  'carFingerprint': CHRYSLER.PACIFICA_2018,
+  #  'enableCamera': True,
+  #},
+  "9f7a7e50a51fb9db|2019-01-17--18-34-21": {
+    'carFingerprint': CHRYSLER.JEEP_CHEROKEE,
+    'enableCamera': True,
+  },
+  "192a598e34926b1e|2019-04-04--13-27-39": {
+    'carFingerprint': CHRYSLER.JEEP_CHEROKEE_2019,
+    'enableCamera': True,
+  },
+  "f1b4c567731f4a1b|2018-04-18--11-29-37": {
+    'carFingerprint': FORD.FUSION,
+    'enableCamera': False,
+  },
+  "f1b4c567731f4a1b|2018-04-30--10-15-35": {
+    'carFingerprint': FORD.FUSION,
+    'enableCamera': True,
+  },
+  "7ed9cdf8d0c5f43e|2018-05-17--09-31-36": {
+    'carFingerprint': GM.CADILLAC_CT6,
+    'enableCamera': True,
+  },
+  "265007255e794bce|2018-11-24--22-08-31": {
+    'carFingerprint': GM.CADILLAC_ATS,
+    'enableCamera': True,
+  },
+  "c950e28c26b5b168|2018-05-30--22-03-41": {
+    'carFingerprint': GM.VOLT,
+    'enableCamera': True,
+  },
+  # TODO: use another route that has radar data at start
+  "aadda448b49c99ad|2018-10-25--17-16-22": {
+    'carFingerprint': GM.MALIBU,
+    'enableCamera': True,
+  },
+  "49c73650e65ff465|2018-11-19--16-58-04": {
+    'carFingerprint': GM.HOLDEN_ASTRA,
+    'enableCamera': True,
+  },
+  "7cc2a8365b4dd8a9|2018-12-02--12-10-44": {
+    'carFingerprint': GM.ACADIA,
+    'enableCamera': True,
+  },
+  "aa20e335f61ba898|2018-12-17--21-10-37": {
+    'carFingerprint': GM.BUICK_REGAL,
+    'enableCamera': False,
+  },
+  "aa20e335f61ba898|2019-02-05--16-59-04": {
+    'carFingerprint': GM.BUICK_REGAL,
+    'enableCamera': True,
+  },
+  "7d44af5b7a1b2c8e|2017-09-16--01-50-07": {
+    'carFingerprint': HONDA.CIVIC,
+    'enableCamera': True,
+  },
+  "c9d60e5e02c04c5c|2018-01-08--16-01-49": {
+    'carFingerprint': HONDA.CRV,
+    'enableCamera': True,
+  },
+  "1851183c395ef471|2018-05-31--18-07-21": {
+    'carFingerprint': HONDA.CRV_5G,
+    'enableCamera': True,
+  },
+  "232585b7784c1af4|2019-04-08--14-12-14": {
+    'carFingerprint': HONDA.CRV_HYBRID,
+    'enableCamera': True,
+  },
+  "2ac95059f70d76eb|2018-02-05--15-03-29": {
+    'carFingerprint': HONDA.ACURA_ILX,
+    'enableCamera': True,
+  },
+  "21aa231dee2a68bd|2018-01-30--04-54-41": {
+    'carFingerprint': HONDA.ODYSSEY,
+    'enableCamera': True,
+  },
+  "81722949a62ea724|2019-03-29--15-51-26": {
+    'carFingerprint': HONDA.ODYSSEY_CHN,
+    'enableCamera': False,
+  },
+  "81722949a62ea724|2019-04-06--15-19-25": {
+    'carFingerprint': HONDA.ODYSSEY_CHN,
+    'enableCamera': True,
+  },
+  "5a2cfe4bb362af9e|2018-02-02--23-41-07": {
+    'carFingerprint': HONDA.ACURA_RDX,
+    'enableCamera': True,
+  },
+  "3e9592a1c78a3d63|2018-02-08--20-28-24": {
+    'carFingerprint': HONDA.PILOT,
+    'enableCamera': True,
+  },
+  "34a84d2b765df688|2018-08-28--12-41-00": {
+    'carFingerprint': HONDA.PILOT_2019,
+    'enableCamera': True,
+  },
+  "900ad17e536c3dc7|2018-04-12--22-02-36": {
+    'carFingerprint': HONDA.RIDGELINE,
+    'enableCamera': True,
+  },
+  "f1b4c567731f4a1b|2018-06-06--14-43-46": {
+    'carFingerprint': HONDA.ACCORD,
+    'enableCamera': True,
+  },
+  "1582e1dc57175194|2018-08-15--07-46-07": {
+    'carFingerprint': HONDA.ACCORD_15,
+    'enableCamera': True,
+  },
+  # TODO: This doesnt fingerprint because the fingerprint overlaps with the Insight
+  # "690c4c9f9f2354c7|2018-09-15--17-36-05": {
+  #   'carFingerprint': HONDA.ACCORDH,
+  #   'enableCamera': True,
+  # },
+  "1632088eda5e6c4d|2018-06-07--08-03-18": {
+    'carFingerprint': HONDA.CIVIC_BOSCH,
+    'enableCamera': True,
+  },
+  #"18971a99f3f2b385|2018-11-14--19-09-31": {
+  #  'carFingerprint': HONDA.INSIGHT,
+  #  'enableCamera': True,
+  #},
+  "38bfd238edecbcd7|2018-08-22--09-45-44": {
+    'carFingerprint': HYUNDAI.SANTA_FE,
+    'enableCamera': False,
+  },
+  "38bfd238edecbcd7|2018-08-29--22-02-15": {
+    'carFingerprint': HYUNDAI.SANTA_FE,
+    'enableCamera': True,
+  },
+  "a893a80e5c5f72c8|2019-01-14--20-02-59": {
+    'carFingerprint': HYUNDAI.GENESIS,
+    'enableCamera': True,
+  },
+  "9d5fb4f0baa1b3e1|2019-01-14--17-45-59": {
+    'carFingerprint': HYUNDAI.KIA_SORENTO,
+    'enableCamera': True,
+  },
+  "215cd70e9c349266|2018-11-25--22-22-12": {
+    'carFingerprint': HYUNDAI.KIA_STINGER,
+    'enableCamera': True,
+  },
+  "31390e3eb6f7c29a|2019-01-23--08-56-00": {
+    'carFingerprint': HYUNDAI.KIA_OPTIMA,
+    'enableCamera': True,
+  },
+  "53ac3251e03f95d7|2019-01-10--13-43-32": {
+    'carFingerprint': HYUNDAI.ELANTRA,
+    'enableCamera': True,
+  },
+  "f7b6be73e3dfd36c|2019-05-12--18-07-16": {
+    'carFingerprint': TOYOTA.AVALON,
+    'enableCamera': False,
+    'enableDsu': False,
+  },
+  "f7b6be73e3dfd36c|2019-05-11--22-34-20": {
+    'carFingerprint': TOYOTA.AVALON,
+    'enableCamera': True,
+    'enableDsu': False,
+  },
+  "b0f5a01cf604185c|2018-01-26--00-54-32": {
+    'carFingerprint': TOYOTA.COROLLA,
+    'enableCamera': True,
+    'enableDsu': True,
+  },
+  "b0f5a01cf604185c|2018-01-26--10-54-38": {
+    'carFingerprint': TOYOTA.COROLLA,
+    'enableCamera': True,
+    'enableDsu': False,
+  },
+  "b0f5a01cf604185c|2018-01-26--10-59-31": {
+    'carFingerprint': TOYOTA.COROLLA,
+    'enableCamera': False,
+    'enableDsu': False,
+  },
+  "5f5afb36036506e4|2019-05-14--02-09-54": {
+    'carFingerprint': TOYOTA.COROLLA_TSS2,
+    'enableCamera': True,
+    'enableDsu': True,
+  },
+  "56fb1c86a9a86404|2017-11-10--10-18-43": {
+    'carFingerprint': TOYOTA.PRIUS,
+    'enableCamera': True,
+    'enableDsu': True,
+  },
+  "b0f5a01cf604185c|2017-12-18--20-32-32": {
+    'carFingerprint': TOYOTA.RAV4,
+    'enableCamera': True,
+    'enableDsu': True,
+    'enableGasInterceptor': False,
+  },
+  "b0c9d2329ad1606b|2019-04-02--13-24-43": {
+    'carFingerprint': TOYOTA.RAV4,
+    'enableCamera': True,
+    'enableDsu': True,
+    'enableGasInterceptor': True,
+  },
+  "cdf2f7de565d40ae|2019-04-25--03-53-41": {
+    'carFingerprint': TOYOTA.RAV4_TSS2,
+    'enableCamera': True,
+    'enableDsu': True,
+  },
+  "f49e8041283f2939|2019-05-29--13-48-33": {
+    'carFingerprint': TOYOTA.LEXUS_ESH_TSS2,
+    'enableCamera': False,
+    'enableDsu': False,
+  },
+  "f49e8041283f2939|2019-05-30--11-51-51": {
+    'carFingerprint': TOYOTA.LEXUS_ESH_TSS2,
+    'enableCamera': True,
+    'enableDsu': True,
+  },
+  "b0f5a01cf604185c|2018-02-01--21-12-28": {
+    'carFingerprint': TOYOTA.LEXUS_RXH,
+    'enableCamera': True,
+    'enableDsu': True,
+  },
+  #FIXME: This works sometimes locally, but never in CI. Timing issue?
+  #"b0f5a01cf604185c|2018-01-31--20-11-39": {
+  #  'carFingerprint': TOYOTA.LEXUS_RXH,
+  #  'enableCamera': False,
+  #  'enableDsu': False,
+  #},
+  "8ae193ceb56a0efe|2018-06-18--20-07-32": {
+    'carFingerprint': TOYOTA.RAV4H,
+    'enableCamera': True,
+    'enableDsu': True,
+  },
+  "fd10b9a107bb2e49|2018-07-24--16-32-42": {
+    'carFingerprint': TOYOTA.CHR,
+    'enableCamera': True,
+    'enableDsu': False,
+  },
+  "fd10b9a107bb2e49|2018-07-24--20-32-08": {
+    'carFingerprint': TOYOTA.CHR,
+    'enableCamera': False,
+    'enableDsu': False,
+  },
+  "b4c18bf13d5955da|2018-07-29--13-39-46": {
+    'carFingerprint': TOYOTA.CHRH,
+    'enableCamera': True,
+    'enableDsu': False,
+  },
+  "d2d8152227f7cb82|2018-07-25--13-40-56": {
+    'carFingerprint': TOYOTA.CAMRY,
+    'enableCamera': True,
+    'enableDsu': False,
+  },
+  "fbd011384db5e669|2018-07-26--20-51-48": {
+    'carFingerprint': TOYOTA.CAMRYH,
+    'enableCamera': True,
+    'enableDsu': False,
+  },
+  # TODO: This replay has no good model/video
+  # "c9fa2dd0f76caf23|2018-02-10--13-40-28": {
+  #   'carFingerprint': TOYOTA.CAMRYH,
+  #   'enableCamera': False,
+  #   'enableDsu': False,
+  # },
+  # TODO: missingsome combos for highlander
+  "aa659debdd1a7b54|2018-08-31--11-12-01": {
+    'carFingerprint': TOYOTA.HIGHLANDER,
+    'enableCamera': False,
+    'enableDsu': False,
+  },
+  "362d23d4d5bea2fa|2018-09-02--17-03-55": {
+    'carFingerprint': TOYOTA.HIGHLANDERH,
+    'enableCamera': True,
+    'enableDsu': True,
+  },
+  "eb6acd681135480d|2019-06-20--20-00-00": {
+    'carFingerprint': TOYOTA.SIENNA,
+    'enableCamera': True,
+    'enableDsu': False,
+  },
+  "362d23d4d5bea2fa|2018-08-10--13-31-40": {
+    'carFingerprint': TOYOTA.HIGHLANDERH,
+    'enableCamera': False,
+    'enableDsu': False,
+  },
+  "791340bc01ed993d|2019-03-10--16-28-08": {
+    'carFingerprint': SUBARU.IMPREZA,
+    'enableCamera': True,
+  },
+  # Tesla route, should result in mock car
+  "07cb8a788c31f645|2018-06-17--18-50-29": {
+    'carFingerprint': MOCK.MOCK,
+  },
+  ## Route with no can data, should result in mock car. This is not supported anymore
+  #"bfa17080b080f3ec|2018-06-28--23-27-47": {
+  #  'carFingerprint': MOCK.MOCK,
+  #},
+}
+
+passive_routes = [
+  "07cb8a788c31f645|2018-06-17--18-50-29",
+  #"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",
+]
+
+if __name__ == "__main__":
+
+  results = {}
+  for route, checks in routes.items():
+
+    if route not in public_routes:
+      print "route not public", route
+      continue
+
+    get_route_logs(route)
+
+    for _ in range(3):
+      shutil.rmtree('/data/params')
+      manager.gctx = {}
+      params = Params()
+      params.manager_start()
+      if route in passive_routes:
+        params.put("Passive", "1")
+      else:
+        params.put("Passive", "0")
+
+      print "testing ", route, " ", checks['carFingerprint']
+      print "Preparing processes"
+      manager.prepare_managed_process("radard")
+      manager.prepare_managed_process("controlsd")
+      manager.prepare_managed_process("plannerd")
+      print "Starting processes"
+      manager.start_managed_process("radard")
+      manager.start_managed_process("controlsd")
+      manager.start_managed_process("plannerd")
+      time.sleep(2)
+
+      # Start unlogger
+      print "Start unlogger"
+      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)
+
+      print "Check sockets"
+      controls_state_result = wait_for_socket('controlsState', timeout=30)
+      radarstate_result = wait_for_socket('radarState', timeout=30)
+      plan_result = wait_for_socket('plan', timeout=30)
+
+      if route not in passive_routes:  # TODO The passive routes have very flaky models
+        path_plan_result = wait_for_socket('pathPlan', timeout=30)
+      else:
+        path_plan_result = True
+
+      carstate_result = wait_for_socket('carState', timeout=30)
+
+      print "Check if everything is running"
+      running = manager.get_running()
+      controlsd_running = running['controlsd'].is_alive()
+      radard_running = running['radard'].is_alive()
+      plannerd_running = running['plannerd'].is_alive()
+
+      manager.kill_managed_process("controlsd")
+      manager.kill_managed_process("radard")
+      manager.kill_managed_process("plannerd")
+      os.killpg(os.getpgid(unlogger.pid), signal.SIGTERM)
+
+      sockets_ok = all([
+        controls_state_result, radarstate_result, plan_result, path_plan_result, carstate_result,
+        controlsd_running, radard_running, plannerd_running
+      ])
+      params_ok = True
+      failures = []
+
+      if not controlsd_running:
+        failures.append('controlsd')
+      if not radard_running:
+        failures.append('radard')
+      if not radarstate_result:
+        failures.append('radarState')
+      if not controls_state_result:
+        failures.append('controlsState')
+      if not plan_result:
+        failures.append('plan')
+      if not path_plan_result:
+        failures.append('pathPlan')
+
+      try:
+        car_params = car.CarParams.from_bytes(params.get("CarParams"))
+        for k, v in checks.items():
+          if not v == getattr(car_params, k):
+            params_ok = False
+            failures.append(k)
+      except:
+        params_ok = False
+
+      if sockets_ok and params_ok:
+        print "Success"
+        results[route] = True, failures
+        break
+      else:
+        print "Failure"
+        results[route] = False, failures
+
+      time.sleep(2)
+
+  print results
+  params.put("Passive", "0")   # put back not passive to not leave the params in an unintended state
+  if not all(passed for passed, _ in results.values()):
+    print "TEST FAILED"
+    sys.exit(1)
+  else:
+    print "TEST SUCESSFUL"

selfdrive/test/tests/process_replay/.gitignore

@@ -0,0 +1,2 @@
+*.bz2
+diff.txt

selfdrive/test/tests/process_replay/README.md

@@ -0,0 +1,15 @@
+# process replay
+
+Process replay is a regression test designed to identify any changes in the output of a process. This test replays a segment through individual processes and compares the output to a known good replay. Each make is represented in the test with a segment.
+
+If the test fails, make sure that you didn't unintentionally change anything. If there are intentional changes, the reference logs will be updated.
+
+Use `test_processes.py` to run the test locally.
+
+Currently the following processes are tested:
+
+* controlsd
+* radard
+* plannerd
+* calibrationd
+

selfdrive/test/tests/process_replay/compare_logs.py

@@ -0,0 +1,45 @@
+#!/usr/bin/env python2
+import bz2
+import os
+import sys
+
+import dictdiffer
+if "CI" in os.environ:
+  tqdm = lambda x: x
+else:
+  from tqdm import tqdm
+
+from tools.lib.logreader import LogReader
+
+
+def save_log(dest, log_msgs):
+  dat = ""
+  for msg in log_msgs:
+    dat += msg.as_builder().to_bytes()
+  dat = bz2.compress(dat)
+
+  with open(dest, "w") as f:
+   f.write(dat)
+
+def compare_logs(log1, log2, ignore=[]):
+  assert len(log1) == len(log2), "logs are not same length"
+
+  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()
+
+    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)
+      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

@@ -0,0 +1,185 @@
+#!/usr/bin/env python2
+import gc
+import os
+import time
+
+if "CI" in os.environ:
+  tqdm = lambda x: x
+else:
+  from tqdm import tqdm
+
+from cereal import car
+from selfdrive.car.car_helpers import get_car
+import selfdrive.manager as manager
+import selfdrive.messaging as messaging
+from common.params import Params
+from selfdrive.services import service_list
+from collections import namedtuple
+
+ProcessConfig = namedtuple('ProcessConfig', ['proc_name', 'pub_sub', 'ignore', 'init_callback', 'should_recv_callback'])
+
+def fingerprint(msgs, pub_socks, sub_socks):
+  print "start fingerprinting"
+  manager.prepare_managed_process("logmessaged")
+  manager.start_managed_process("logmessaged")
+
+  can = pub_socks["can"]
+  logMessage = messaging.sub_sock(service_list["logMessage"].port)
+
+  time.sleep(1)
+  messaging.drain_sock(logMessage)
+
+  # controlsd waits for a health packet before fingerprinting
+  msg = messaging.new_message()
+  msg.init("health")
+  pub_socks["health"].send(msg.to_bytes())
+
+  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")
+  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)
+
+  canmsgs = filter(lambda msg: msg.which() == "can", msgs)
+  for m in canmsgs[:200]:
+    can.send(m.as_builder().to_bytes())
+  _, CP = get_car(logcan, 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 []
+
+  # hyundai and subaru don't have radar
+  radar_msgs = {"honda": [0x445], "toyota": [0x19f, 0x22f], "gm": [0x475],
+                "hyundai": [], "chrysler": [0x2d4], "subaru": []}.get(CP.carName, None)
+
+  if radar_msgs is None:
+    raise NotImplementedError
+
+  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": []
+    },
+    ignore=["logMonoTime", "controlsState.startMonoTime", "controlsState.cumLagMs"],
+    init_callback=fingerprint,
+    should_recv_callback=None,
+  ),
+  ProcessConfig(
+    proc_name="radard",
+    pub_sub={
+      "can": ["radarState", "liveTracks"],
+      "liveParameters":  [], "controlsState":  [], "model":  [],
+    },
+    ignore=["logMonoTime", "radarState.cumLagMs"],
+    init_callback=get_car_params,
+    should_recv_callback=radar_rcv_callback,
+  ),
+  ProcessConfig(
+    proc_name="plannerd",
+    pub_sub={
+      "model": ["pathPlan"], "radarState": ["plan"],
+      "carState": [], "controlsState": [], "liveParameters": [],
+    },
+    ignore=["logMonoTime", "valid", "plan.processingDelay"],
+    init_callback=get_car_params,
+    should_recv_callback=plannerd_rcv_callback,
+  ),
+  ProcessConfig(
+    proc_name="calibrationd",
+    pub_sub={
+      "cameraOdometry": ["liveCalibration"]
+    },
+    ignore=["logMonoTime"],
+    init_callback=get_car_params,
+    should_recv_callback=None,
+  ),
+]
+
+def replay_process(cfg, lr):
+  gc.disable()  # gc can occasionally cause canparser to timeout
+
+  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)
+
+  all_msgs = sorted(lr, key=lambda msg: msg.logMonoTime)
+  pub_msgs = filter(lambda msg: msg.which() in pub_socks.keys(), all_msgs)
+
+  params = Params()
+  params.manager_start()
+  params.put("Passive", "0")
+
+  manager.gctx = {}
+  manager.prepare_managed_process(cfg.proc_name)
+  manager.start_managed_process(cfg.proc_name)
+  time.sleep(3)   # Wait for started process to be ready
+
+  if cfg.init_callback is not None:
+    cfg.init_callback(all_msgs, pub_socks, sub_socks)
+
+  CP = car.CarParams.from_bytes(params.get("CarParams", block=True))
+
+  log_msgs = []
+  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()]
+
+    pub_socks[msg.which()].send(msg.as_builder().to_bytes())
+
+    if len(recv_socks):
+      # TODO: add timeout
+      for sock in recv_socks:
+        m = messaging.recv_one(sub_socks[sock])
+
+        # 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())
+
+  gc.enable()
+  manager.kill_managed_process(cfg.proc_name)
+  return log_msgs
+

selfdrive/test/tests/process_replay/ref_commit

@@ -0,0 +1 @@
+e3388c62ffb80f4b3ca8721da56a581a93c44e79

selfdrive/test/tests/process_replay/test_processes.py

@@ -0,0 +1,118 @@
+#!/usr/bin/env python2
+import os
+import requests
+import sys
+import tempfile
+
+from selfdrive.test.tests.process_replay.compare_logs import compare_logs
+from selfdrive.test.tests.process_replay.process_replay import replay_process, CONFIGS
+from tools.lib.logreader import LogReader
+
+segments = [
+  "0375fdf7b1ce594d|2019-06-13--08-32-25--3", # HONDA.ACCORD
+  "99c94dc769b5d96e|2019-08-03--14-19-59--2", # HONDA.CIVIC
+  "cce908f7eb8db67d|2019-08-02--15-09-51--3", # TOYOTA.COROLLA_TSS2
+  "7ad88f53d406b787|2019-07-09--10-18-56--8", # GM.VOLT
+  "704b2230eb5190d6|2019-07-06--19-29-10--0", # HYUNDAI.KIA_SORENTO
+  "b6e1317e1bfbefa6|2019-07-06--04-05-26--5", # CHRYSLER.JEEP_CHEROKEE
+  "7873afaf022d36e2|2019-07-03--18-46-44--0", # SUBARU.IMPREZA
+]
+
+def get_segment(segment_name):
+  route_name, segment_num = segment_name.rsplit("--", 1)
+  rlog_url = "https://commadataci.blob.core.windows.net/openpilotci/%s/%s/rlog.bz2" \
+                % (route_name.replace("|", "/"), segment_num)
+  r = requests.get(rlog_url)
+  if r.status_code != 200:
+    return None
+
+  with tempfile.NamedTemporaryFile(delete=False, suffix=".bz2") as f:
+    f.write(r.content)
+    return f.name
+
+if __name__ == "__main__":
+
+  process_replay_dir = os.path.dirname(os.path.abspath(__file__))
+  ref_commit_fn = os.path.join(process_replay_dir, "ref_commit")
+
+  if not os.path.isfile(ref_commit_fn):
+    print "couldn't find reference commit"
+    sys.exit(1)
+
+  ref_commit = open(ref_commit_fn).read().strip()
+  print "***** testing against commit %s *****" % ref_commit
+
+  results = {}
+  for segment in segments:
+    print "***** testing route segment %s *****\n" % segment
+
+    results[segment] = {}
+
+    rlog_fn = get_segment(segment)
+
+    if rlog_fn is None:
+      print "failed to get segment %s" % segment
+      sys.exit(1)
+
+    lr = LogReader(rlog_fn)
+
+    for cfg in CONFIGS:
+      log_msgs = replay_process(cfg, lr)
+
+      log_fn = os.path.join(process_replay_dir, "%s_%s_%s.bz2" % (segment, cfg.proc_name, ref_commit))
+
+      if not os.path.isfile(log_fn):
+        url = "https://commadataci.blob.core.windows.net/openpilotci/"
+        req = requests.get(url + os.path.basename(log_fn))
+        if req.status_code != 200:
+          results[segment][cfg.proc_name] = "failed to download comparison log"
+          continue
+
+        with tempfile.NamedTemporaryFile(suffix=".bz2") as f:
+          f.write(req.content)
+          f.flush()
+          f.seek(0)
+          cmp_log_msgs = list(LogReader(f.name))
+      else:
+        cmp_log_msgs = list(LogReader(log_fn))
+
+      diff = compare_logs(cmp_log_msgs, log_msgs, cfg.ignore)
+      results[segment][cfg.proc_name] = diff
+    os.remove(rlog_fn)
+
+  failed = False
+  with open(os.path.join(process_replay_dir, "diff.txt"), "w") as f:
+    f.write("***** tested against commit %s *****\n" % ref_commit)
+
+    for segment, result in results.items():
+      f.write("***** differences for segment %s *****\n" % segment)
+      print "***** results for segment %s *****" % segment
+
+      for proc, diff in result.items():
+        f.write("*** process: %s ***\n" % proc)
+        print "\t%s" % proc
+
+        if isinstance(diff, str):
+          print "\t\t%s" % diff
+          failed = True
+        elif len(diff):
+          cnt = {}
+          for d in diff:
+            f.write("\t%s\n" % str(d))
+
+            k = str(d[1])
+            cnt[k] = 1 if k not in cnt else cnt[k] + 1
+
+          for k, v in sorted(cnt.items()):
+            print "\t\t%s: %s" % (k, v)
+          failed = True
+
+    if failed:
+      print "TEST FAILED"
+    else:
+      print "TEST SUCCEEDED"
+
+  print "\n\nTo update the reference logs for this test run:"
+  print "./update_refs.py"
+
+  sys.exit(int(failed))

selfdrive/test/tests/process_replay/update_refs.py

@@ -0,0 +1,42 @@
+#!/usr/bin/env python2
+import os
+import sys
+
+from selfdrive.test.openpilotci_upload import upload_file
+from selfdrive.test.tests.process_replay.compare_logs import save_log
+from selfdrive.test.tests.process_replay.process_replay import replay_process, CONFIGS
+from selfdrive.test.tests.process_replay.test_processes import segments, get_segment
+from selfdrive.version import get_git_commit
+from tools.lib.logreader import LogReader
+
+if __name__ == "__main__":
+
+  no_upload = "--no-upload" in sys.argv
+
+  process_replay_dir = os.path.dirname(os.path.abspath(__file__))
+  ref_commit_fn = os.path.join(process_replay_dir, "ref_commit")
+
+  ref_commit = get_git_commit()
+  with open(ref_commit_fn, "w") as f:
+    f.write(ref_commit)
+
+  for segment in segments:
+    rlog_fn = get_segment(segment)
+
+    if rlog_fn is None:
+      print "failed to get segment %s" % segment
+      sys.exit(1)
+
+    lr = LogReader(rlog_fn)
+
+    for cfg in CONFIGS:
+      log_msgs = replay_process(cfg, lr)
+      log_fn = os.path.join(process_replay_dir, "%s_%s_%s.bz2" % (segment, cfg.proc_name, ref_commit))
+      save_log(log_fn, log_msgs)
+
+      if not no_upload:
+        upload_file(log_fn, os.path.basename(log_fn))
+        os.remove(log_fn)
+    os.remove(rlog_fn)
+
+  print "done"

selfdrive/ui/slplay.c

@@ -111,7 +111,7 @@ void slplay_destroy() {
   (*engine)->Destroy(engine);
 }
 
-void slplay_stop (uri_player* player, char **error) {
+void slplay_stop(uri_player* player, char **error) {
   SLPlayItf playInterface = player->playInterface;
   SLresult result;
 

selfdrive/ui/spinner/spinner

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:11953df252f1cd63d7cd2101508b89c96f5de073b3181c4262f1a26d3c871ca1
+oid sha256:7a4f45d19c1d85792e4c48a60f7d9beb0264ce187ff5edccf8532fda37fe831d
 size 239976

selfdrive/ui/spinner/spinner.c

@@ -35,7 +35,7 @@ int main(int argc, char** argv) {
   NVGcontext *vg = nvgCreateGLES3(NVG_ANTIALIAS | NVG_STENCIL_STROKES);
   assert(vg);
 
-  int font = nvgCreateFont(vg, "Bold", "../../assets/OpenSans-SemiBold.ttf");
+  int font = nvgCreateFont(vg, "Bold", "../../assets/fonts/opensans_semibold.ttf");
   assert(font >= 0);
 
   int spinner_img = nvgCreateImage(vg, "../../assets/img_spinner_track.png", 0);

selfdrive/ui/ui.c

@@ -107,6 +107,8 @@ const mat3 intrinsic_matrix = (mat3){{
   0.,   0.,   1.
 }};
 
+typedef enum cereal_CarControl_HUDControl_AudibleAlert AudibleAlert;
+
 typedef struct UIScene {
   int frontview;
   int fullview;
@@ -161,8 +163,6 @@ typedef struct UIScene {
 
   // Used to show gps planner status
   bool gps_planner_active;
-
-  bool is_playing_alert;
 } UIScene;
 
 typedef struct {
@@ -253,6 +253,7 @@ typedef struct UIState {
   int awake_timeout;
 
   int volume_timeout;
+  int alert_sound_timeout;
   int speed_lim_off_timeout;
   int is_metric_timeout;
   int longitudinal_control_timeout;
@@ -265,7 +266,7 @@ typedef struct UIState {
   float speed_lim_off;
   bool is_ego_over_limit;
   char alert_type[64];
-  char alert_sound[64];
+  AudibleAlert alert_sound;
   int alert_size;
   float alert_blinking_alpha;
   bool alert_blinked;
@@ -431,25 +432,25 @@ static const mat4 full_to_wide_frame_transform = {{
 }};
 
 typedef struct {
-  const char* name;
+  AudibleAlert alert;
   const char* uri;
   bool loop;
 } sound_file;
 
 sound_file sound_table[] = {
-  { "chimeDisengage", "../assets/sounds/disengaged.wav", false },
+  { cereal_CarControl_HUDControl_AudibleAlert_chimeDisengage, "../assets/sounds/disengaged.wav", false },
-  { "chimeEngage", "../assets/sounds/engaged.wav", false },
+  { cereal_CarControl_HUDControl_AudibleAlert_chimeEngage, "../assets/sounds/engaged.wav", false },
-  { "chimeWarning1", "../assets/sounds/warning_1.wav", false },
+  { cereal_CarControl_HUDControl_AudibleAlert_chimeWarning1, "../assets/sounds/warning_1.wav", false },
-  { "chimeWarning2", "../assets/sounds/warning_2.wav", false },
+  { cereal_CarControl_HUDControl_AudibleAlert_chimeWarning2, "../assets/sounds/warning_2.wav", false },
-  { "chimeWarningRepeat", "../assets/sounds/warning_2.wav", true },
+  { cereal_CarControl_HUDControl_AudibleAlert_chimeWarningRepeat, "../assets/sounds/warning_2.wav", true },
-  { "chimeError", "../assets/sounds/error.wav", false },
+  { cereal_CarControl_HUDControl_AudibleAlert_chimeError, "../assets/sounds/error.wav", false },
-  { "chimePrompt", "../assets/sounds/error.wav", false },
+  { cereal_CarControl_HUDControl_AudibleAlert_chimePrompt, "../assets/sounds/error.wav", false },
-  { NULL, NULL, false },
+  { cereal_CarControl_HUDControl_AudibleAlert_none, NULL, false },
 };
 
-sound_file* get_sound_file_by_name(const char* name) {
+sound_file* get_sound_file(AudibleAlert alert) {
-  for (sound_file *s = sound_table; s->name != NULL; s++) {
+  for (sound_file *s = sound_table; s->alert != cereal_CarControl_HUDControl_AudibleAlert_none; s++) {
-    if (strcmp(s->name, name) == 0) {
+    if (s->alert == alert) {
       return s;
     }
   }
@@ -462,7 +463,7 @@ void ui_sound_init(char **error) {
   slplay_setup(error);
   if (*error) return;
 
-  for (sound_file *s = sound_table; s->name != NULL; s++) {
+  for (sound_file *s = sound_table; s->alert != cereal_CarControl_HUDControl_AudibleAlert_none; s++) {
     slplay_create_player_for_uri(s->uri, error);
     if (*error) return;
   }
@@ -502,13 +503,13 @@ static void ui_init(UIState *s) {
   s->vg = nvgCreateGLES3(NVG_ANTIALIAS | NVG_STENCIL_STROKES | NVG_DEBUG);
   assert(s->vg);
 
-  s->font_courbd = nvgCreateFont(s->vg, "courbd", "../assets/courbd.ttf");
+  s->font_courbd = nvgCreateFont(s->vg, "courbd", "../assets/fonts/courbd.ttf");
   assert(s->font_courbd >= 0);
-  s->font_sans_regular = nvgCreateFont(s->vg, "sans-regular", "../assets/OpenSans-Regular.ttf");
+  s->font_sans_regular = nvgCreateFont(s->vg, "sans-regular", "../assets/fonts/opensans_regular.ttf");
   assert(s->font_sans_regular >= 0);
-  s->font_sans_semibold = nvgCreateFont(s->vg, "sans-semibold", "../assets/OpenSans-SemiBold.ttf");
+  s->font_sans_semibold = nvgCreateFont(s->vg, "sans-semibold", "../assets/fonts/opensans_semibold.ttf");
   assert(s->font_sans_semibold >= 0);
-  s->font_sans_bold = nvgCreateFont(s->vg, "sans-bold", "../assets/OpenSans-Bold.ttf");
+  s->font_sans_bold = nvgCreateFont(s->vg, "sans-bold", "../assets/fonts/opensans_bold.ttf");
   assert(s->font_sans_bold >= 0);
 
   assert(s->img_wheel >= 0);
@@ -1218,8 +1219,8 @@ static void ui_draw_vision_speedlimit(UIState *s) {
   if (is_speedlim_valid && s->is_ego_over_limit) {
     nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 255));
   }
-  nvgText(s->vg, viz_speedlim_x+viz_speedlim_w/2 + (is_speedlim_valid ? 6 : 0), viz_speedlim_y + (is_speedlim_valid ? 50 : 45), "SPEED", NULL);
+  nvgText(s->vg, viz_speedlim_x+viz_speedlim_w/2 + (is_speedlim_valid ? 6 : 0), viz_speedlim_y + (is_speedlim_valid ? 50 : 45), "SMART", NULL);
-  nvgText(s->vg, viz_speedlim_x+viz_speedlim_w/2 + (is_speedlim_valid ? 6 : 0), viz_speedlim_y + (is_speedlim_valid ? 90 : 85), "LIMIT", NULL);
+  nvgText(s->vg, viz_speedlim_x+viz_speedlim_w/2 + (is_speedlim_valid ? 6 : 0), viz_speedlim_y + (is_speedlim_valid ? 90 : 85), "SPEED", NULL);
 
   // Draw Speed Text
   nvgFontFace(s->vg, "sans-bold");
@@ -1414,7 +1415,7 @@ static void ui_draw_vision_footer(UIState *s) {
   ui_draw_vision_face(s);
 
 #ifdef SHOW_SPEEDLIMIT
-  ui_draw_vision_map(s);
+  // ui_draw_vision_map(s);
 #endif
 }
 
@@ -1633,26 +1634,28 @@ void handle_message(UIState *s, void *which) {
 
     s->scene.decel_for_model = datad.decelForModel;
 
-    if (datad.alertSound.str && datad.alertSound.str[0] != '\0' && strcmp(s->alert_type, datad.alertType.str) != 0) {
+    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[0] != '\0') {
+      if (s->alert_sound != cereal_CarControl_HUDControl_AudibleAlert_none) {
-        sound_file* active_sound = get_sound_file_by_name(s->alert_sound);
+        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_by_name(datad.alertSound.str);
+      sound_file* sound = get_sound_file(datad.alertSound);
       slplay_play(sound->uri, sound->loop, &error);
       if(error) {
         LOGW("error playing sound: %s", error);
       }
 
-      snprintf(s->alert_sound, sizeof(s->alert_sound), "%s", datad.alertSound.str);
+      s->alert_sound = datad.alertSound;
       snprintf(s->alert_type, sizeof(s->alert_type), "%s", datad.alertType.str);
-    } else if ((!datad.alertSound.str || datad.alertSound.str[0] == '\0') && s->alert_sound[0] != '\0') {
+    } 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_by_name(s->alert_sound);
+      sound_file* sound = get_sound_file(s->alert_sound);
 
       char* error = NULL;
 
@@ -1661,7 +1664,7 @@ void handle_message(UIState *s, void *which) {
         LOGW("error stopping sound: %s", error);
       }
       s->alert_type[0] = '\0';
-      s->alert_sound[0] = '\0';
+      s->alert_sound = cereal_CarControl_HUDControl_AudibleAlert_none;
     }
 
     if (datad.alertText1.str) {
@@ -1798,8 +1801,6 @@ void handle_message(UIState *s, void *which) {
   } else if (eventd.which == cereal_Event_liveMapData) {
     struct cereal_LiveMapData datad;
     cereal_read_LiveMapData(&datad, eventd.liveMapData);
-    s->scene.speedlimit = datad.speedLimit;
-    s->scene.speedlimit_valid = datad.speedLimitValid;
     s->scene.map_valid = datad.mapValid;
   }
   capn_free(&ctx);
@@ -2230,7 +2231,10 @@ int main(int argc, char* argv[]) {
 
   float smooth_brightness = BRIGHTNESS_B;
 
-  set_volume(s, 13);
+  const int MIN_VOLUME = LEON ? 12 : 8;
+  const int MAX_VOLUME = LEON ? 15 : 13;
+
+  set_volume(s, MIN_VOLUME);
 #ifdef DEBUG_FPS
   vipc_t1 = millis_since_boot();
   double t1 = millis_since_boot();
@@ -2304,10 +2308,24 @@ int main(int argc, char* argv[]) {
     if (s->volume_timeout > 0) {
       s->volume_timeout--;
     } else {
-      int volume = min(13, 11 + s->scene.v_ego / 15);  // up one notch every 15 m/s, starting at 11
+      int volume = min(MAX_VOLUME, MIN_VOLUME + s->scene.v_ego / 5);  // up one notch every 5 m/s
       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;
+
+      slplay_stop_uri(sound->uri, &error);
+      if(error) {
+        LOGW("error stopping sound: %s", error);
+      }
+      s->alert_sound = cereal_CarControl_HUDControl_AudibleAlert_none;
+    }
+
     read_param_bool_timeout(&s->is_metric, "IsMetric", &s->is_metric_timeout);
     read_param_bool_timeout(&s->longitudinal_control, "LongitudinalControl", &s->longitudinal_control_timeout);
     read_param_bool_timeout(&s->limit_set_speed, "LimitSetSpeed", &s->limit_set_speed_timeout);

selfdrive/visiond/build_from_src.mk

@@ -46,11 +46,10 @@ else
   LIBYUV_FLAGS = -I$(PHONELIBS)/libyuv/include
   LIBYUV_LIBS = $(PHONELIBS)/libyuv/x64/lib/libyuv.a
 
-  ZMQ_FLAGS = -I$(PHONELIBS)/zmq/aarch64/include
+  ZMQ_FLAGS = -I$(PHONELIBS)/zmq/x64/include
-  ZMQ_LIBS = -l:libczmq.a -l:libzmq.a -lsodium
+  ZMQ_LIBS = -L$(PHONELIBS)/zmq/x64/lib/ -l:libczmq.a -l:libzmq.a
 
   OPENCL_LIBS = -lOpenCL
-  UUID_LIBS = -luuid
 
   TF_FLAGS = -I$(EXTERNAL)/tensorflow/include
   TF_LIBS = -L$(EXTERNAL)/tensorflow/lib -ltensorflow \

selfdrive/visiond/models/driving.cc

@@ -4,36 +4,32 @@
 #include <unistd.h>
 
 #ifdef QCOM
-  #include <eigen3/Eigen/Dense>
+#include <eigen3/Eigen/Dense>
 #else
-  #include <Eigen/Dense>
+#include <Eigen/Dense>
 #endif
 
 #include "common/timing.h"
 #include "driving.h"
 
-#ifdef MEDMODEL
+#define MODEL_WIDTH 512
-  #define MODEL_WIDTH 512
+#define MODEL_HEIGHT 256
-  #define MODEL_HEIGHT 256
+#define MODEL_NAME "driving_model_dlc"
-  #define MODEL_NAME "driving_model_dlc"
-#else
-  #define MODEL_WIDTH 320
-  #define MODEL_HEIGHT 160
-  #define MODEL_NAME "driving_model_dlc"
-#endif
 
 #define LEAD_MDN_N 5 // probs for 5 groups
 #define MDN_VALS 4 // output xyva for each lead group
 #define SELECTION 3 //output 3 group (lead now, in 2s and 6s)
 #define MDN_GROUP_SIZE 11
 #define SPEED_BUCKETS 100
-#define OUTPUT_SIZE ((MODEL_PATH_DISTANCE*2) + (2*(MODEL_PATH_DISTANCE*2 + 1)) + MDN_GROUP_SIZE*LEAD_MDN_N + SELECTION + SPEED_BUCKETS)
+#define OUTPUT_SIZE ((MODEL_PATH_DISTANCE*2) + (2*(MODEL_PATH_DISTANCE*2 + 1)) + MDN_GROUP_SIZE*LEAD_MDN_N + SELECTION)
 #ifdef TEMPORAL
   #define TEMPORAL_SIZE 512
 #else
   #define TEMPORAL_SIZE 0
 #endif
 
+// #define DUMP_YUV
+
 Eigen::Matrix<float, MODEL_PATH_DISTANCE, POLYFIT_DEGREE> vander;
 
 void model_init(ModelState* s, cl_device_id device_id, cl_context context, int temporal) {
@@ -70,6 +66,13 @@ ModelData model_eval_frame(ModelState* s, cl_command_queue q,
 
   float *net_input_buf = model_input_prepare(&s->in, q, yuv_cl, width, height, transform);
 
+  #ifdef DUMP_YUV
+    FILE *dump_yuv_file = fopen("/sdcard/dump.yuv", "wb");
+    fwrite(net_input_buf, MODEL_HEIGHT*MODEL_WIDTH*3/2, sizeof(float), dump_yuv_file);
+    fclose(dump_yuv_file);
+    assert(1==2);
+  #endif
+
   //printf("readinggggg \n");
   //FILE *f = fopen("goof_frame", "r");
   //fread(net_input_buf, sizeof(float), MODEL_HEIGHT*MODEL_WIDTH*3/2, f);
@@ -84,7 +87,7 @@ ModelData model_eval_frame(ModelState* s, cl_command_queue q,
   net_outputs.left_lane = &s->output[MODEL_PATH_DISTANCE*2];
   net_outputs.right_lane = &s->output[MODEL_PATH_DISTANCE*2 + MODEL_PATH_DISTANCE*2 + 1];
   net_outputs.lead = &s->output[MODEL_PATH_DISTANCE*2 + (MODEL_PATH_DISTANCE*2 + 1)*2];
-  net_outputs.speed = &s->output[OUTPUT_SIZE - SPEED_BUCKETS];
+  //net_outputs.speed = &s->output[OUTPUT_SIZE - SPEED_BUCKETS];
 
   ModelData model = {0};
 
@@ -111,7 +114,11 @@ ModelData model_eval_frame(ModelState* s, cl_command_queue q,
 
   const double max_dist = 140.0;
   const double max_rel_vel = 10.0;
-  // current lead
+  // Every output distribution from the MDN includes the probabilties
+  // of it representing a current lead car, a lead car in 2s
+  // or a lead car in 4s
+
+  // Find the distribution that corresponds to the current lead
   int mdn_max_idx = 0;
   for (int i=1; i<LEAD_MDN_N; i++) {
     if (net_outputs.lead[i*MDN_GROUP_SIZE + 8] > net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + 8]) {
@@ -128,7 +135,7 @@ ModelData model_eval_frame(ModelState* s, cl_command_queue q,
   model.lead.rel_a = net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + 3];
   model.lead.rel_a_std = softplus(net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + MDN_VALS + 3]);
 
-  // lead in 2s 
+  // Find the distribution that corresponds to the lead in 2s
   mdn_max_idx = 0;
   for (int i=1; i<LEAD_MDN_N; i++) {
     if (net_outputs.lead[i*MDN_GROUP_SIZE + 9] > net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + 9]) {
@@ -150,20 +157,20 @@ ModelData model_eval_frame(ModelState* s, cl_command_queue q,
   for (int i=0; i < SPEED_PERCENTILES; i++) {
     model.speed[i] = ((float) SPEED_BUCKETS)/2.0;
   }
-  float sum = 0;
+  //float sum = 0;
-  for (int idx = 0; idx < SPEED_BUCKETS; idx++) {
+  //for (int idx = 0; idx < SPEED_BUCKETS; idx++) {
-    sum += net_outputs.speed[idx];
+  //  sum += net_outputs.speed[idx];
-    int idx_percentile = (sum + .05) * SPEED_PERCENTILES;
+  //  int idx_percentile = (sum + .05) * SPEED_PERCENTILES;
-    if (idx_percentile < SPEED_PERCENTILES ){
+  //  if (idx_percentile < SPEED_PERCENTILES ){
-      model.speed[idx_percentile] = ((float)idx)/2.0;
+  //    model.speed[idx_percentile] = ((float)idx)/2.0;
-    }
+  //  }
-  }
+  //}
   // make sure no percentiles are skipped
-  for (int i=SPEED_PERCENTILES-1; i > 0; i--){
+  //for (int i=SPEED_PERCENTILES-1; i > 0; i--){
-    if (model.speed[i-1] > model.speed[i]){
+  //  if (model.speed[i-1] > model.speed[i]){
-      model.speed[i-1] = model.speed[i];
+  //    model.speed[i-1] = model.speed[i];
-    }
+  //  }
-  }
+  //}
   return model;
 }
 

selfdrive/visiond/models/driving.h

@@ -2,7 +2,6 @@
 #define MODEL_H
 
 // gate this here
-#define MEDMODEL
 #define TEMPORAL
 
 #include "common/mat.h"

selfdrive/visiond/models/monitoring.cc

@@ -42,6 +42,8 @@ MonitoringResult monitoring_eval_frame(MonitoringState* s, cl_command_queue q,
   memcpy(&ret.face_prob, &s->output[12], sizeof ret.face_prob);
   memcpy(&ret.left_eye_prob, &s->output[21], sizeof ret.left_eye_prob);
   memcpy(&ret.right_eye_prob, &s->output[30], sizeof ret.right_eye_prob);
+  memcpy(&ret.left_blink_prob, &s->output[31], sizeof ret.right_eye_prob);
+  memcpy(&ret.right_blink_prob, &s->output[32], sizeof ret.right_eye_prob);
   return ret;
 }
 

selfdrive/visiond/models/monitoring.h

@@ -9,7 +9,7 @@ extern "C" {
 #endif
 
 #define OUTPUT_SIZE_DEPRECATED 8
-#define OUTPUT_SIZE 31
+#define OUTPUT_SIZE 33
 
 typedef struct MonitoringResult {
   float descriptor_DEPRECATED[OUTPUT_SIZE_DEPRECATED - 1];
@@ -20,6 +20,8 @@ typedef struct MonitoringResult {
   float face_prob;
   float left_eye_prob;
   float right_eye_prob;
+  float left_blink_prob;
+  float right_blink_prob;
 } MonitoringResult;
 
 typedef struct MonitoringState {

selfdrive/visiond/runners/run.h

@@ -7,8 +7,9 @@
 #ifdef QCOM
   #define DefaultRunModel SNPEModel
 #else
-  #include "tfmodel.h"
+#define DefaultRunModel SNPEModel
-  #define DefaultRunModel TFModel
+  /* #include "tfmodel.h" */
+  /* #define DefaultRunModel TFModel */
 #endif
 
 #endif

selfdrive/visiond/visiond.cc

@@ -26,6 +26,12 @@
 #include <capnp/serialize.h>
 #include <jpeglib.h>
 
+#ifdef QCOM
+#include <eigen3/Eigen/Dense>
+#else
+#include <Eigen/Dense>
+#endif
+
 #include "common/version.h"
 #include "common/util.h"
 #include "common/timing.h"
@@ -45,6 +51,7 @@
 #include "cameras/camera_frame_stream.h"
 #endif
 
+
 // 3 models
 #include "models/driving.h"
 #include "models/monitoring.h"
@@ -58,7 +65,7 @@
 
 #define UI_BUF_COUNT 4
 
-//#define DUMP_RGB
+// #define DUMP_RGB
 
 //#define DEBUG_DRIVER_MONITOR
 
@@ -741,6 +748,8 @@ void* monitoring_thread(void *arg) {
         framed.setFaceProb(res.face_prob);
         framed.setLeftEyeProb(res.left_eye_prob);
         framed.setRightEyeProb(res.right_eye_prob);
+        framed.setLeftBlinkProb(res.left_blink_prob);
+        framed.setRightBlinkProb(res.right_blink_prob);
 
 
         auto words = capnp::messageToFlatArray(msg);
@@ -895,6 +904,8 @@ void* processing_thread(void *arg) {
 #endif
 
 #ifdef DUMP_RGB
+  s->rgb_width = s->frame_width;
+  s->rgb_height = s->frame_height;
   FILE *dump_rgb_file = fopen("/sdcard/dump.rgb", "wb");
 #endif
 
@@ -959,6 +970,8 @@ void* processing_thread(void *arg) {
 #ifdef DUMP_RGB
     if (cnt % 20 == 0) {
       fwrite(bgr_ptr, s->rgb_buf_size, 1, dump_rgb_file);
+      LOG("%d x %d", s->rgb_width, s->rgb_height);
+      assert(1==2);
     }
 #endif
 
@@ -1202,6 +1215,24 @@ void* live_thread(void *arg) {
   zpoller_t *poller = zpoller_new(liveCalibration_sock, terminate, NULL);
   assert(poller);
 
+  /*
+     import numpy as np
+     from common.transformations.model import medmodel_frame_from_road_frame
+     medmodel_frame_from_ground = medmodel_frame_from_road_frame[:, (0, 1, 3)]
+     ground_from_medmodel_frame = np.linalg.inv(medmodel_frame_from_ground)
+  */
+  Eigen::Matrix<float, 3, 3> ground_from_medmodel_frame;
+  ground_from_medmodel_frame <<
+    0.00000000e+00, 0.00000000e+00, 1.00000000e+00,
+    -1.09890110e-03, 0.00000000e+00, 2.81318681e-01,
+    -1.84808520e-20, 9.00738606e-04,-4.28751576e-02;
+
+  Eigen::Matrix<float, 3, 3> eon_intrinsics;
+  eon_intrinsics <<
+    910.0, 0.0, 582.0,
+    0.0, 910.0, 437.0,
+    0.0,   0.0,   1.0;
+
   while (!do_exit) {
     zsock_t *which = (zsock_t*)zpoller_wait(poller, -1);
     if (which == terminate || which == NULL) {
@@ -1226,15 +1257,25 @@ void* live_thread(void *arg) {
 
     if (event.isLiveCalibration()) {
       pthread_mutex_lock(&s->transform_lock);
-#ifdef MEDMODEL
+
-      auto wm2 = event.getLiveCalibration().getWarpMatrixBig();
+      auto extrinsic_matrix = event.getLiveCalibration().getExtrinsicMatrix();
-#else
+      Eigen::Matrix<float, 3, 4> extrinsic_matrix_eigen;
-      auto wm2 = event.getLiveCalibration().getWarpMatrix2();
+      for (int i = 0; i < 4*3; i++){
-#endif
+        extrinsic_matrix_eigen(i / 4, i % 4) = extrinsic_matrix[i];
-      assert(wm2.size() == 3*3);
+      }
+
+      auto camera_frame_from_road_frame = eon_intrinsics * extrinsic_matrix_eigen;
+      Eigen::Matrix<float, 3, 3> camera_frame_from_ground;
+      camera_frame_from_ground.col(0) = camera_frame_from_road_frame.col(0);
+      camera_frame_from_ground.col(1) = camera_frame_from_road_frame.col(1);
+      camera_frame_from_ground.col(2) = camera_frame_from_road_frame.col(3);
+
+      auto warp_matrix = camera_frame_from_ground * ground_from_medmodel_frame;
+
       for (int i=0; i<3*3; i++) {
-        s->cur_transform.v[i] = wm2[i];
+        s->cur_transform.v[i] = warp_matrix(i / 3, i % 3);
       }
+
       s->run_model = true;
       pthread_mutex_unlock(&s->transform_lock);
     }