From 02cedeadd97ddef18639d2f0dce5e32422016e55 Mon Sep 17 00:00:00 2001 From: Vehicle Researcher Date: Tue, 13 Aug 2019 01:36:45 +0000 Subject: [PATCH] openpilot v0.6.3 release old-commit-hash: d5f9caa82d80cdcc7f1b7748f2cf3ccbf94f82a3 --- Dockerfile.openpilot | 4 +- README.md | 6 +- RELEASES.md | 11 + apk/ai.comma.plus.offroad.apk | 4 +- common/api/__init__.py | 19 + installer/updater/Makefile | 14 +- installer/updater/update.json | 8 +- installer/updater/updater | 4 +- installer/updater/updater.cc | 113 ++-- models/driving_model.dlc | 4 +- models/monitoring_model.dlc | 4 +- run_docker_tests.sh | 4 +- selfdrive/assets/Roboto-Bold.ttf | 3 - selfdrive/assets/{ => fonts}/courbd.ttf | 0 .../opensans_bold.ttf} | 0 .../opensans_regular.ttf} | 0 .../opensans_semibold.ttf} | 0 selfdrive/assets/sounds/disengaged.wav | 4 +- selfdrive/assets/sounds/engaged.wav | 4 +- selfdrive/assets/sounds/error.wav | 4 +- selfdrive/assets/sounds/warning_1.wav | 4 +- selfdrive/assets/sounds/warning_2.wav | 4 +- selfdrive/athena/athenad.py | 9 +- selfdrive/can/tests/test_packer_honda.py | 8 +- selfdrive/can/tests/test_packer_toyota.py | 12 +- selfdrive/car/chrysler/carcontroller.py | 19 +- selfdrive/car/chrysler/chryslercan.py | 10 - selfdrive/car/chrysler/chryslercan_test.py | 1 - selfdrive/car/chrysler/interface.py | 3 +- selfdrive/car/chrysler/values.py | 2 +- selfdrive/car/gm/carcontroller.py | 20 +- selfdrive/car/gm/gmcan.py | 4 - selfdrive/car/gm/interface.py | 7 +- selfdrive/car/gm/values.py | 21 - selfdrive/car/honda/carcontroller.py | 14 +- selfdrive/car/honda/carstate.py | 5 + selfdrive/car/honda/hondacan.py | 11 +- selfdrive/car/honda/interface.py | 14 +- selfdrive/car/honda/values.py | 26 - selfdrive/car/hyundai/interface.py | 2 +- selfdrive/car/hyundai/values.py | 5 +- selfdrive/car/toyota/carcontroller.py | 23 +- selfdrive/car/toyota/interface.py | 33 +- selfdrive/car/toyota/toyotacan.py | 6 +- selfdrive/common/version.h | 2 +- selfdrive/controls/controlsd.py | 33 +- selfdrive/controls/lib/alertmanager.py | 9 +- selfdrive/controls/lib/alerts.py | 38 +- selfdrive/controls/lib/driver_monitor.py | 109 ++-- selfdrive/controls/lib/lane_planner.py | 74 +++ selfdrive/controls/lib/latcontrol_helpers.py | 27 - selfdrive/controls/lib/latcontrol_indi.py | 2 +- selfdrive/controls/lib/latcontrol_lqr.py | 72 +++ selfdrive/controls/lib/latcontrol_pid.py | 2 +- .../controls/lib/lateral_mpc/generator.cpp | 35 +- .../controls/lib/lateral_mpc/lateral_mpc.c | 15 +- .../lateral_mpc/lib_mpc_export/acado_common.h | 2 +- .../lib_mpc_export/acado_integrator.c | 4 +- .../lateral_mpc/lib_mpc_export/acado_solver.c | 4 +- .../controls/lib/lateral_mpc/libmpc_py.py | 4 +- selfdrive/controls/lib/model_parser.py | 66 --- selfdrive/controls/lib/pathplanner.py | 47 +- selfdrive/controls/lib/planner.py | 48 +- selfdrive/controls/lib/radar_helpers.py | 30 +- selfdrive/controls/radard.py | 11 +- selfdrive/controls/tests/test_lateral_mpc.py | 20 +- selfdrive/locationd/calibrationd.py | 10 +- selfdrive/locationd/locationd_yawrate.cc | 19 +- selfdrive/locationd/paramsd.cc | 9 +- selfdrive/loggerd/uploader.py | 21 +- selfdrive/manager.py | 5 +- selfdrive/registration.py | 4 + selfdrive/test/openpilotci_upload.py | 22 + selfdrive/test/plant/plant.py | 29 +- selfdrive/test/test_car_models_openpilot.py | 494 ++++++++++++++++++ selfdrive/test/tests/__init__.py | 0 .../test/tests/process_replay/.gitignore | 2 + selfdrive/test/tests/process_replay/README.md | 15 + .../test/tests/process_replay/__init__.py | 0 .../test/tests/process_replay/compare_logs.py | 45 ++ .../tests/process_replay/process_replay.py | 185 +++++++ .../test/tests/process_replay/ref_commit | 1 + .../tests/process_replay/test_processes.py | 118 +++++ .../test/tests/process_replay/update_refs.py | 42 ++ selfdrive/ui/slplay.c | 2 +- selfdrive/ui/spinner/spinner | 2 +- selfdrive/ui/spinner/spinner.c | 2 +- selfdrive/ui/ui.c | 88 ++-- selfdrive/visiond/build_from_src.mk | 5 +- selfdrive/visiond/models/driving.cc | 65 ++- selfdrive/visiond/models/driving.h | 1 - selfdrive/visiond/models/monitoring.cc | 2 + selfdrive/visiond/models/monitoring.h | 4 +- selfdrive/visiond/runners/run.h | 5 +- selfdrive/visiond/visiond.cc | 57 +- 95 files changed, 1755 insertions(+), 626 deletions(-) delete mode 100644 selfdrive/assets/Roboto-Bold.ttf rename selfdrive/assets/{ => fonts}/courbd.ttf (100%) rename selfdrive/assets/{OpenSans-Bold.ttf => fonts/opensans_bold.ttf} (100%) rename selfdrive/assets/{OpenSans-Regular.ttf => fonts/opensans_regular.ttf} (100%) rename selfdrive/assets/{OpenSans-SemiBold.ttf => fonts/opensans_semibold.ttf} (100%) create mode 100644 selfdrive/controls/lib/lane_planner.py create mode 100644 selfdrive/controls/lib/latcontrol_lqr.py delete mode 100644 selfdrive/controls/lib/model_parser.py create mode 100755 selfdrive/test/openpilotci_upload.py create mode 100755 selfdrive/test/test_car_models_openpilot.py create mode 100644 selfdrive/test/tests/__init__.py create mode 100644 selfdrive/test/tests/process_replay/.gitignore create mode 100644 selfdrive/test/tests/process_replay/README.md create mode 100644 selfdrive/test/tests/process_replay/__init__.py create mode 100755 selfdrive/test/tests/process_replay/compare_logs.py create mode 100755 selfdrive/test/tests/process_replay/process_replay.py create mode 100644 selfdrive/test/tests/process_replay/ref_commit create mode 100755 selfdrive/test/tests/process_replay/test_processes.py create mode 100755 selfdrive/test/tests/process_replay/update_refs.py diff --git a/Dockerfile.openpilot b/Dockerfile.openpilot index edf6d47860..6be64c1311 100644 --- a/Dockerfile.openpilot +++ b/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 diff --git a/README.md b/README.md index a4e47fc444..abcc299795 100644 --- a/README.md +++ b/README.md @@ -72,6 +72,7 @@ Supported Cars | GMC3 | Acadia Denali 2018 | Adaptive Cruise | Yes | Yes | 0mph | 7mph | Custom7| | Holden3 | Astra 2017 | Adaptive Cruise | Yes | Yes | 0mph | 7mph | Custom7| | 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 | Yes2| 20mph1| 0mph | Toyota | | Toyota | Avalon 2017-18 | All | Yes | Yes2| 20mph1| 0mph | Toyota | | Toyota | Camry 2018-19 | All | Yes | Stock | 0mph5 | 0mph | Toyota | +| Toyota | Camry Hybrid 2018-19 | All | Yes | Stock | 0mph5 | 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 | Yes2| 20mph1| 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 | Yes2| 0mph | 0mph | Toyota | -| Toyota | Highlander Hybrid 2018 | All | Yes | Yes2| 0mph | 0mph | Toyota | +| Toyota | Highlander Hybrid 2017-19| All | Yes | Yes2| 0mph | 0mph | Toyota | | Toyota | Prius 2016 | TSS-P | Yes | Yes2| 0mph | 0mph | Toyota | | Toyota | Prius 2017-19 | All | Yes | Yes2| 0mph | 0mph | Toyota | | Toyota | Prius Prime 2017-19 | All | Yes | Yes2| 0mph | 0mph | Toyota | | Toyota | Rav4 2016 | TSS-P | Yes | Yes2| 20mph1| 0mph | Toyota | | Toyota | Rav4 2017-18 | All | Yes | Yes2| 20mph1| 0mph | Toyota | | Toyota | Rav4 2019 | All | Yes | Yes | 0mph | 0mph | Toyota | +| Toyota | Rav4 Hybrid 2016 | TSS-P | Yes | Yes2| 0mph | 0mph | Toyota | | Toyota | Rav4 Hybrid 2017-18 | All | Yes | Yes2| 0mph | 0mph | Toyota | | Toyota | Sienna 2018 | All | Yes | Yes2| 0mph | 0mph | Toyota | diff --git a/RELEASES.md b/RELEASES.md index f28977e922..d847bb3f05 100644 --- a/RELEASES.md +++ b/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! diff --git a/apk/ai.comma.plus.offroad.apk b/apk/ai.comma.plus.offroad.apk index ea0be63058..5c4aefa295 100644 --- a/apk/ai.comma.plus.offroad.apk +++ b/apk/ai.comma.plus.offroad.apk @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:3bf24d5dd606d364e339c8823a5b71b0a5271716b551a187ca8a8ea457a275d0 -size 17042107 +oid sha256:9eba15d8e77847b242a83fbd43dd67a5bd5d0c6d76f4f11f6fd6820621f69771 +size 17096653 diff --git a/common/api/__init__.py b/common/api/__init__.py index 2dc2ffa990..e457272a12 100644 --- a/common/api/__init__.py +++ b/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/" diff --git a/installer/updater/Makefile b/installer/updater/Makefile index d9444e6c59..d252fc2aa0 100644 --- a/installer/updater/Makefile +++ b/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 $@)' diff --git a/installer/updater/update.json b/installer/updater/update.json index 827a44b10d..2715fcaccc 100644 --- a/installer/updater/update.json +++ b/installer/updater/update.json @@ -1,7 +1,7 @@ { - "ota_url": "https://commadist.azureedge.net/neosupdate/ota-signed-c992abb59cbaf6588f51055db52db619061107851773fc8480acb8bb5d77a28f.zip", - "ota_hash": "c992abb59cbaf6588f51055db52db619061107851773fc8480acb8bb5d77a28f", - "recovery_url": "https://commadist.azureedge.net/neosupdate/recovery-af099a84cfd7b91266090779238ac358278948dcde2dcfa0fbca6e8397366f0a.img", + "ota_url": "https://commadist.azureedge.net/neosupdate/ota-signed-4db25072191d24e204a816d73ac9e8c727822a26ed3baf01ecae18167fa2eb11.zip", + "ota_hash": "4db25072191d24e204a816d73ac9e8c727822a26ed3baf01ecae18167fa2eb11", + "recovery_url": "https://commadist.azureedge.net/neosupdate/recovery-31ef14206d3102edf18fb7417ef32ba2d9f37dd2f4443e234c374a70d1bf4662.img", "recovery_len": 15136044, - "recovery_hash": "af099a84cfd7b91266090779238ac358278948dcde2dcfa0fbca6e8397366f0a" + "recovery_hash": "31ef14206d3102edf18fb7417ef32ba2d9f37dd2f4443e234c374a70d1bf4662" } diff --git a/installer/updater/updater b/installer/updater/updater index 401c339a39..c7bad1c690 100755 --- a/installer/updater/updater +++ b/installer/updater/updater @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:743ba86c3b4d643272c11eba28e2b0a8824c542caa613a94ff6a2f01c37e54a7 -size 2501656 +oid sha256:da43154c2563cfda1af50018a412a6105338c4fafc021f8b3c7442fcec514d44 +size 2464536 diff --git a/installer/updater/updater.cc b/installer/updater/updater.cc index 58fab32413..89ac7f6359 100644 --- a/installer/updater/updater.cc +++ b/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_courbd_end[] asm("_binary_courbd_ttf_end"); +extern const uint8_t bin_opensans_regular[] asm("_binary_opensans_regular_ttf_start"); +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_h = 250; + b_y = 720; + 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) { - nvgFontSize(vg, 96.0f); + void draw_ack_screen(const char *title, const char *message, const char *button, const char *altbutton) { nvgFillColor(vg, nvgRGBA(255,255,255,255)); - nvgTextAlign(vg, NVG_ALIGN_LEFT | NVG_ALIGN_BASELINE); - nvgTextBox(vg, 50, 100, fb_w-100, message, NULL); + nvgTextAlign(vg, NVG_ALIGN_CENTER | NVG_ALIGN_BASELINE); + + 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_height = 15; + int progress_y = 520; + int progress_height = 50; - int powerprompt_y = 512; - nvgText(vg, fb_w/2, powerprompt_y, "Ensure EON is connected to power", NULL); + int powerprompt_y = 312; + 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" - "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.", - "continue", - "wifi"); + 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. Existing data on device should not be lost.", + "Continue", + "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); diff --git a/models/driving_model.dlc b/models/driving_model.dlc index 64be984b4b..7715a96c0a 100644 --- a/models/driving_model.dlc +++ b/models/driving_model.dlc @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:32aef4d710c994e0c8a019b64eb2bd495f493be0dc0daa9f312f7dfeeb7f1568 -size 14761531 +oid sha256:f5fb0036eaaa6136b8b605c24ab255de45b14ce827af538e15c47a04a6b200af +size 14438998 diff --git a/models/monitoring_model.dlc b/models/monitoring_model.dlc index aafcc0f4bd..c1880fccc3 100644 --- a/models/monitoring_model.dlc +++ b/models/monitoring_model.dlc @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:eb047afc34b4b5b9888dc1dfc796d4f83fc695734decac5f6a6057a70e468623 -size 544528 +oid sha256:9fdd514ad8a38876468adf9fa106f511150c077223c13eb1cc3ae939bc414564 +size 601956 diff --git a/run_docker_tests.sh b/run_docker_tests.sh index 10d1bbc835..dfee5f8669 100755 --- a/run_docker_tests.sh +++ b/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' diff --git a/selfdrive/assets/Roboto-Bold.ttf b/selfdrive/assets/Roboto-Bold.ttf deleted file mode 100644 index 5ff22906a6..0000000000 --- a/selfdrive/assets/Roboto-Bold.ttf +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:ef2ab0e402d5cb9de893e263a2c44e57f57fec3974b0d981bfe84dec3dae83a1 -size 162464 diff --git a/selfdrive/assets/courbd.ttf b/selfdrive/assets/fonts/courbd.ttf similarity index 100% rename from selfdrive/assets/courbd.ttf rename to selfdrive/assets/fonts/courbd.ttf diff --git a/selfdrive/assets/OpenSans-Bold.ttf b/selfdrive/assets/fonts/opensans_bold.ttf similarity index 100% rename from selfdrive/assets/OpenSans-Bold.ttf rename to selfdrive/assets/fonts/opensans_bold.ttf diff --git a/selfdrive/assets/OpenSans-Regular.ttf b/selfdrive/assets/fonts/opensans_regular.ttf similarity index 100% rename from selfdrive/assets/OpenSans-Regular.ttf rename to selfdrive/assets/fonts/opensans_regular.ttf diff --git a/selfdrive/assets/OpenSans-SemiBold.ttf b/selfdrive/assets/fonts/opensans_semibold.ttf similarity index 100% rename from selfdrive/assets/OpenSans-SemiBold.ttf rename to selfdrive/assets/fonts/opensans_semibold.ttf diff --git a/selfdrive/assets/sounds/disengaged.wav b/selfdrive/assets/sounds/disengaged.wav index f62fccde79..4a0dcd2ca3 100644 --- a/selfdrive/assets/sounds/disengaged.wav +++ b/selfdrive/assets/sounds/disengaged.wav @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:c898025ae57cbc55d29af9381fe65fd2db4704beb46c606411c4522d059bfe0b -size 428188 +oid sha256:f888f84627b4b69395cdf3f8bedf7a6a50c13253b30b7ddc2eda010c8be16b66 +size 40662 diff --git a/selfdrive/assets/sounds/engaged.wav b/selfdrive/assets/sounds/engaged.wav index 9bb3ef3272..a6f8289699 100644 --- a/selfdrive/assets/sounds/engaged.wav +++ b/selfdrive/assets/sounds/engaged.wav @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:bd26959d1a73071764378885fbb1a2941cfd4a15650361a5b6fbeccf81900def -size 159998 +oid sha256:113afc45a281e491a771bdd749f1375b96b174e9c74a8dff5ab1cd83f3cad406 +size 40654 diff --git a/selfdrive/assets/sounds/error.wav b/selfdrive/assets/sounds/error.wav index e14a9e288c..8b2d61f767 100644 --- a/selfdrive/assets/sounds/error.wav +++ b/selfdrive/assets/sounds/error.wav @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:6c0cf1b6fcdb5489146b2973aed01c4deb2bed34d9610e39b8dc3596d6e1e9af -size 71030 +oid sha256:301eef864fa7b306527828bb9e8295208c08c24382026391a9206b19aca58a4e +size 40650 diff --git a/selfdrive/assets/sounds/warning_1.wav b/selfdrive/assets/sounds/warning_1.wav index cc34f03977..666a74a69c 100644 --- a/selfdrive/assets/sounds/warning_1.wav +++ b/selfdrive/assets/sounds/warning_1.wav @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:d39d45560063a9b3f0b2c473cda5ea1e052b38ee3fd7dcbd4465043b37f661ed -size 128798 +oid sha256:9cffe1d8cd2dec399fb3fce5ce2833e0405bd44191d3a24d6085592cd8d31dd5 +size 21468 diff --git a/selfdrive/assets/sounds/warning_2.wav b/selfdrive/assets/sounds/warning_2.wav index 42028a32a0..ff4a4da902 100644 --- a/selfdrive/assets/sounds/warning_2.wav +++ b/selfdrive/assets/sounds/warning_2.wav @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:8249c685cab9896799290f28a53cc3751dd3fbf31c300e03fd039cd129e6c03c -size 119446 +oid sha256:62580fd60d5784505f4f7697e5a5011cf0ae1f3c92ed1104700159bfbcd7de3e +size 52800 diff --git a/selfdrive/athena/athenad.py b/selfdrive/athena/athenad.py index d6d429d92b..6aa84d106e 100755 --- a/selfdrive/athena/athenad.py +++ b/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/" + dongle_id + ws_uri = ATHENA_HOST + "/ws/v2/" + 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 diff --git a/selfdrive/can/tests/test_packer_honda.py b/selfdrive/can/tests/test_packer_honda.py index b1744155be..b6e779e40c 100644 --- a/selfdrive/can/tests/test_packer_honda.py +++ b/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 = 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_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, 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), - 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)) 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) diff --git a/selfdrive/can/tests/test_packer_toyota.py b/selfdrive/can/tests/test_packer_toyota.py index 42e36f3171..f5f0e8a7f4 100644 --- a/selfdrive/can/tests/test_packer_toyota.py +++ b/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 = create_ui_command(self.cp, 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, 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)) diff --git a/selfdrive/car/chrysler/carcontroller.py b/selfdrive/car/chrysler/carcontroller.py index eea51f8f67..dac43ca95d 100644 --- a/selfdrive/car/chrysler/carcontroller.py +++ b/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_chimes + create_wheel_buttons 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) diff --git a/selfdrive/car/chrysler/chryslercan.py b/selfdrive/car/chrysler/chryslercan.py index f803e6094c..d710262e91 100644 --- a/selfdrive/car/chrysler/chryslercan.py +++ b/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 diff --git a/selfdrive/car/chrysler/chryslercan_test.py b/selfdrive/car/chrysler/chryslercan_test.py index 4bcb346ba2..54b845342f 100644 --- a/selfdrive/car/chrysler/chryslercan_test.py +++ b/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 diff --git a/selfdrive/car/chrysler/interface.py b/selfdrive/car/chrysler/interface.py index 6cc4061d6c..c43c6c08ab 100755 --- a/selfdrive/car/chrysler/interface.py +++ b/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.hudControl.audibleAlert) + c.actuators, c.cruiseControl.cancel, c.hudControl.visualAlert) return can_sends diff --git a/selfdrive/car/chrysler/values.py b/selfdrive/car/chrysler/values.py index f132881542..2e74d70239 100644 --- a/selfdrive/car/chrysler/values.py +++ b/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}, diff --git a/selfdrive/car/gm/carcontroller.py b/selfdrive/car/gm/carcontroller.py index a2ffac61f0..da820864f0 100644 --- a/selfdrive/car/gm/carcontroller.py +++ b/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 diff --git a/selfdrive/car/gm/gmcan.py b/selfdrive/car/gm/gmcan.py index 64fd84f4ac..6919de3bd1 100644 --- a/selfdrive/car/gm/gmcan.py +++ b/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: diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py index 449b8ae5dc..f5f641836f 100755 --- a/selfdrive/car/gm/interface.py +++ b/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, \ - chime, chime_count, c.hudControl.visualAlert) + c.hudControl.leadVisible, c.hudControl.visualAlert) self.frame += 1 return can_sends diff --git a/selfdrive/car/gm/values.py b/selfdrive/car/gm/values.py index b41919acb9..1aa5a64b13 100644 --- a/selfdrive/car/gm/values.py +++ b/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: diff --git a/selfdrive/car/honda/carcontroller.py b/selfdrive/car/honda/carcontroller.py index de8ae50787..8b96e511c2 100644 --- a/selfdrive/car/honda/carcontroller.py +++ b/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_alert, snd_beep, snd_chime): + hud_v_cruise, hud_show_lanes, hud_show_car, hud_alert): # *** 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: diff --git a/selfdrive/car/honda/carstate.py b/selfdrive/car/honda/carstate.py index 07ceb46c3e..abd622d154 100644 --- a/selfdrive/car/honda/carstate.py +++ b/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): diff --git a/selfdrive/car/honda/hondacan.py b/selfdrive/car/honda/hondacan.py index a4c0e98542..723368a92d 100644 --- a/selfdrive/car/honda/hondacan.py +++ b/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)) diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py index 9e7b64885e..d192f3a34f 100755 --- a/selfdrive/car/honda/interface.py +++ b/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, - snd_beep=snd_beep, - snd_chime=snd_chime) + hud_alert=hud_alert) self.frame += 1 return can_sends diff --git a/selfdrive/car/honda/values.py b/selfdrive/car/honda/values.py index c33300c43f..c7acaffc82 100644 --- a/selfdrive/car/honda/values.py +++ b/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" diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index c367324dac..be92371f1d 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/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) diff --git a/selfdrive/car/hyundai/values.py b/selfdrive/car/hyundai/values.py index 1756976e94..6380ca4463 100644 --- a/selfdrive/car/hyundai/values.py +++ b/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 diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index c6ffe0ea1e..28ea8cf6ec 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/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: - sound1 = 1 - elif audible_alert != AudibleAlert.none: - # TODO: find a way to send single chimes - sound2 = 1 - - return steer, fcw, sound1, sound2 + return steer, fcw 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, - left_line, right_line, lead, left_lane_depart, right_lane_depart): + pcm_cancel_cmd, hud_alert, forwarding_camera, left_line, + 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) - steer, fcw, sound1, sound2 = alert_out + alert_out = process_hud_alert(hud_alert) + 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)) diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index dc3fe54499..1e3b61624d 100755 --- a/selfdrive/car/toyota/interface.py +++ b/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.pid.kf = 0.00006 # full torque for 10 deg at 80mph means 0.00007818594 + ret.lateralTuning.init('lqr') + + 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, - c.hudControl.leftLaneVisible, c.hudControl.rightLaneVisible, c.hudControl.leadVisible, + self.forwarding_camera, c.hudControl.leftLaneVisible, + c.hudControl.rightLaneVisible, c.hudControl.leadVisible, c.hudControl.leftLaneDepart, c.hudControl.rightLaneDepart) self.frame += 1 diff --git a/selfdrive/car/toyota/toyotacan.py b/selfdrive/car/toyota/toyotacan.py index 7de38c8914..35ba674528 100644 --- a/selfdrive/car/toyota/toyotacan.py +++ b/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, - "TWO_BEEPS": sound2, + "REPEATED_BEEPS": 0, + "TWO_BEEPS": 0, "LDA_ALERT": steer, } return packer.make_can_msg("LKAS_HUD", 0, values) diff --git a/selfdrive/common/version.h b/selfdrive/common/version.h index 1148fbd95b..d17343827e 100644 --- a/selfdrive/common/version.h +++ b/selfdrive/common/version.h @@ -1 +1 @@ -#define COMMA_VERSION "0.6.2-release" +#define COMMA_VERSION "0.6.3-release" diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 55ad2093a0..9a463bacae 100755 --- a/selfdrive/controls/controlsd.py +++ b/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, - CS.steeringPressed, CP, VM, path_plan) + actuators.steer, actuators.steerAngle, lac_log = LaC.update(active, CS.vEgo, CS.steeringAngle, CS.steeringRate, CS.steeringTorqueEps, 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: diff --git a/selfdrive/controls/lib/alertmanager.py b/selfdrive/controls/lib/alertmanager.py index df2727e6e7..77196273b6 100644 --- a/selfdrive/controls/lib/alertmanager.py +++ b/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.audible_alert = "none" + self.visual_alert = VisualAlert.none + self.audible_alert = AudibleAlert.none self.alert_rate = 0. if current_alert: diff --git a/selfdrive/controls/lib/alerts.py b/selfdrive/controls/lib/alerts.py index 90a92cb0f6..fbef59fe4d 100644 --- a/selfdrive/controls/lib/alerts.py +++ b/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", diff --git a/selfdrive/controls/lib/driver_monitor.py b/selfdrive/controls/lib/driver_monitor.py index 083d408bfd..3b45e5a063 100644 --- a/selfdrive/controls/lib/driver_monitor.py +++ b/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_PRE_TIME = 20. # a first alert is issued 20s before expiration -_AWARENESS_PROMPT_TIME = 5. # a second alert is issued 5s before start decelerating the car -_DISTRACTED_TIME = 7. -_DISTRACTED_PRE_TIME = 4. -_DISTRACTED_PROMPT_TIME = 2. -# model output refers to center of cropped image, so need to apply the x displacement offset +_AWARENESS_TIME = 90. # 1.5 minutes limit without user touching steering wheels make the car enter a terminal status +_AWARENESS_PRE_TIME_TILL_TERMINAL = 20. # a first alert is issued 20s before expiration +_AWARENESS_PROMPT_TIME_TILL_TERMINAL = 5. # a second alert is issued 5s before start decelerating the car +_DISTRACTED_TIME = 10. +_DISTRACTED_PRE_TIME_TILL_TERMINAL = 7. +_DISTRACTED_PROMPT_TIME_TILL_TERMINAL = 5. + _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_NATURAL_OFFSET = 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) -_STD_THRESHOLD = 0.1 # above this standard deviation consider the measurement invalid +_PITCH_POS_ALLOWANCE = 0.04 # 0.08 # rad, to not be too sensitive on positive pitch +_PITCH_NATURAL_OFFSET = 0.12 # 0.1 # people don't seem to look straight when they drive relaxed, rather a bit up +_YAW_NATURAL_OFFSET = 0.08 # people don't seem to look straight when they drive relaxed, rather a bit to the right (center of car) _DISTRACTED_FILTER_TS = 0.25 # 0.6Hz _VARIANCE_FILTER_TS = 20. # 0.008Hz 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): - if self.monitor_on: - self.threshold_pre = _DISTRACTED_PRE_TIME / _DISTRACTED_TIME - self.threshold_prompt = _DISTRACTED_PROMPT_TIME / _DISTRACTED_TIME + def _set_timers(self, active_monitoring): + if active_monitoring: + # when falling back from passive mode to active mode, reset awareness to avoid false alert + if self.step_change == DT_CTRL / _AWARENESS_TIME: + self.awareness = 1. + 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_prompt = _AWARENESS_PROMPT_TIME / _AWARENESS_TIME + self.threshold_pre = _AWARENESS_PRE_TIME_TILL_TERMINAL / _AWARENESS_TIME + self.threshold_prompt = _AWARENESS_PROMPT_TIME_TILL_TERMINAL / _AWARENESS_TIME self.step_change = DT_CTRL / _AWARENESS_TIME - def _is_driver_distracted(self, pose): - # to be tuned and to learn the driver's normal pose + def _is_driver_distracted(self, pose, blink): + # 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) - # TODO: do something with the eye states and combine them with head pose - return 1 if metric > _METRIC_THRESHOLD else 0 + pose_metric = np.sqrt(yaw_error**2 + pitch_error**2) + + 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 - if (not self.monitor_on or (self.driver_distraction_filter.x > 0.63 and self.driver_distracted and self.face_detected)) and \ + # should always be counting if distracted unless at standstill and reaching orange + if ((not self.monitor_on or (self.monitor_on and not self.face_detected)) or (self.driver_distraction_filter.x > 0.63 and self.driver_distracted and self.face_detected)) and \ not (standstill and self.awareness - self.step_change <= self.threshold_prompt): self.awareness = max(self.awareness - self.step_change, -0.1) diff --git a/selfdrive/controls/lib/lane_planner.py b/selfdrive/controls/lib/lane_planner.py new file mode 100644 index 0000000000..60a24b6d9b --- /dev/null +++ b/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) diff --git a/selfdrive/controls/lib/latcontrol_helpers.py b/selfdrive/controls/lib/latcontrol_helpers.py index c04d820bbe..ff5fa478ac 100644 --- a/selfdrive/controls/lib/latcontrol_helpers.py +++ b/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 diff --git a/selfdrive/controls/lib/latcontrol_indi.py b/selfdrive/controls/lib/latcontrol_indi.py index 31ff6dd3dc..38ba8883b9 100644 --- a/selfdrive/controls/lib/latcontrol_indi.py +++ b/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) diff --git a/selfdrive/controls/lib/latcontrol_lqr.py b/selfdrive/controls/lib/latcontrol_lqr.py new file mode 100644 index 0000000000..8c0e3ec31f --- /dev/null +++ b/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 diff --git a/selfdrive/controls/lib/latcontrol_pid.py b/selfdrive/controls/lib/latcontrol_pid.py index 65a5a8b6d9..5fe456d881 100644 --- a/selfdrive/controls/lib/latcontrol_pid.py +++ b/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) diff --git a/selfdrive/controls/lib/lateral_mpc/generator.cpp b/selfdrive/controls/lib/lateral_mpc/generator.cpp index 523ed8ac84..5f4a9a28df 100644 --- a/selfdrive/controls/lib/lateral_mpc/generator.cpp +++ b/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 l_prob, r_prob, p_prob; + OnlineData d_poly_r0, d_poly_r1, d_poly_r2, d_poly_r3; + 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_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; + auto angle_d = atan(3*d_poly_r0*xx*xx + 2*d_poly_r1*xx + d_poly_r2); // 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_right_lane = r_prob * poly_r + (1 - r_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) * (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 ); diff --git a/selfdrive/controls/lib/lateral_mpc/lateral_mpc.c b/selfdrive/controls/lib/lateral_mpc/lateral_mpc.c index ea6f85b304..d8d29cc61c 100644 --- a/selfdrive/controls/lib/lateral_mpc/lateral_mpc.c +++ b/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_prob, double r_prob, double p_prob, double curvature_factor, double v_ref, double lane_width){ + double l_poly[4], double r_poly[4], double d_poly[4], + 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+11] = p_poly[1]; - acadoVariables.od[i+12] = p_poly[2]; - acadoVariables.od[i+13] = p_poly[3]; + acadoVariables.od[i+10] = d_poly[0]; + acadoVariables.od[i+11] = d_poly[1]; + acadoVariables.od[i+12] = d_poly[2]; + 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+17] = lane_width; + acadoVariables.od[i+16] = lane_width; } diff --git a/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_common.h b/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_common.h index eb43ff0d63..1f42d2786e 100644 --- a/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_common.h +++ b/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 diff --git a/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_integrator.c b/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_integrator.c index b2232747a6..ed53ad0703 100644 --- a/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_integrator.c +++ b/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 -size 18662 +oid sha256:5848ec6e7975d6fee93187e0f41d6cba57cc0ebee6edf63ebddf3c7ad6f8f52c +size 18622 diff --git a/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_solver.c b/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_solver.c index 2d7afc5f03..0754c655bb 100644 --- a/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_solver.c +++ b/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 -size 400285 +oid sha256:a2c030dd09379475b0247609d8a02f161f3e468e85480740d4abcf9c80868de0 +size 390405 diff --git a/selfdrive/controls/lib/lateral_mpc/libmpc_py.py b/selfdrive/controls/lib/lateral_mpc/libmpc_py.py index 6b86d9565a..92c0df8da4 100644 --- a/selfdrive/controls/lib/lateral_mpc/libmpc_py.py +++ b/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_prob, double r_prob, double p_prob, double curvature_factor, double v_ref, double lane_width); + double l_poly[4], double r_poly[4], double d_poly[4], + double l_prob, double r_prob, double curvature_factor, double v_ref, double lane_width); """) libmpc = ffi.dlopen(libmpc_fn) diff --git a/selfdrive/controls/lib/model_parser.py b/selfdrive/controls/lib/model_parser.py deleted file mode 100644 index 9b28c66bb9..0000000000 --- a/selfdrive/controls/lib/model_parser.py +++ /dev/null @@ -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 diff --git a/selfdrive/controls/lib/pathplanner.py b/selfdrive/controls/lib/pathplanner.py index 190f7c843c..faa812ac6f 100644 --- a/selfdrive/controls/lib/pathplanner.py +++ b/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.l_poly = [0., 0., 0., 0.] - self.r_poly = [0., 0., 0., 0.] + self.LP = LanePlanner() 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) - self.p_poly = list(self.MP.p_poly) + + # TODO: Check for active, override, and saturation + # 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, - self.MP.l_prob, self.MP.r_prob, self.MP.p_prob, curvature_factor, v_ego_mpc, self.MP.lane_width) + list(self.LP.l_poly), list(self.LP.r_poly), list(self.LP.d_poly), + 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.dPoly = [float(x) for x in self.MP.d_poly] - plan_send.pathPlan.cPoly = [float(x) for x in self.MP.c_poly] - plan_send.pathPlan.cProb = float(self.MP.c_prob) - plan_send.pathPlan.lPoly = [float(x) for x in self.l_poly] - plan_send.pathPlan.lProb = float(self.MP.l_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.laneWidth = float(self.LP.lane_width) + plan_send.pathPlan.dPoly = [float(x) for x in self.LP.d_poly] + plan_send.pathPlan.lPoly = [float(x) for x in self.LP.l_poly] + plan_send.pathPlan.lProb = float(self.LP.l_prob) + plan_send.pathPlan.rPoly = [float(x) for x in self.LP.r_poly] + plan_send.pathPlan.rProb = float(self.LP.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) diff --git a/selfdrive/controls/lib/planner.py b/selfdrive/controls/lib/planner.py index 2e01a8f4f9..52712d15a7 100755 --- a/selfdrive/controls/lib/planner.py +++ b/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.model_v_kf = KF1D([[0.0],[0.0]], _MODEL_V_A, _MODEL_V_C, _MODEL_V_K) - self.model_v_kf_ready = False + self.path_x = np.arange(192) 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: - self.model_v_kf.x = [[v_ego],[0.0]] - self.model_v_kf_ready = True - - if len(sm['model'].speed): - self.model_v_kf.update(sm['model'].speed[SPEED_PERCENTILE_IDX]) - - if self.params.get("LimitSetSpeedNeural") == "1": - model_speed = self.model_v_kf.x[0][0] + if len(sm['model'].path.poly): + path = list(sm['model'].path.poly) + + # Curvature of polynomial https://en.wikipedia.org/wiki/Curvature#Curvature_of_the_graph_of_a_function + # 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 + # k = y'' / (1 + y'^2)^1.5 + y_p = 3 * path[0] * self.path_x**2 + 2 * path[1] * self.path_x + path[2] + y_pp = 6 * path[0] * self.path_x + 2 * path[1] + 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.aCruise = self.a_cruise - plan_send.plan.vStart = self.v_acc_start - plan_send.plan.aStart = self.a_acc_start - plan_send.plan.vTarget = self.v_acc - plan_send.plan.aTarget = self.a_acc - plan_send.plan.vTargetFuture = self.v_acc_future + plan_send.plan.vCruise = float(self.v_cruise) + plan_send.plan.aCruise = float(self.a_cruise) + plan_send.plan.vStart = float(self.v_acc_start) + plan_send.plan.aStart = float(self.a_acc_start) + plan_send.plan.vTarget = float(self.v_acc) + plan_send.plan.aTarget = float(self.a_acc) + plan_send.plan.vTargetFuture = float(self.v_acc_future) plan_send.plan.hasLead = self.mpc1.prev_lead_status plan_send.plan.longitudinalPlanSource = self.longitudinalPlanSource diff --git a/selfdrive/controls/lib/radar_helpers.py b/selfdrive/controls/lib/radar_helpers.py index 6b3d768c7d..02a38ca2a8 100644 --- a/selfdrive/controls/lib/radar_helpers.py +++ b/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: - self.initted = True - self.aLeadTau = _LEAD_ACCEL_TAU - self.cnt = 1 - self.vision_cnt = 0 - self.vision = False + if self.cnt == 0: 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): diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index 77e9febbcb..84410fa8e1 100755 --- a/selfdrive/controls/radard.py +++ b/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) diff --git a/selfdrive/controls/tests/test_lateral_mpc.py b/selfdrive/controls/tests/test_lateral_mpc.py index 77b37bb740..4bd6571348 100644 --- a/selfdrive/controls/tests/test_lateral_mpc.py +++ b/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() diff --git a/selfdrive/locationd/calibrationd.py b/selfdrive/locationd/calibrationd.py index e3a0b10ad0..2890b0f61d 100755 --- a/selfdrive/locationd/calibrationd.py +++ b/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()) diff --git a/selfdrive/locationd/locationd_yawrate.cc b/selfdrive/locationd/locationd_yawrate.cc index df2c724899..93e706499a 100644 --- a/selfdrive/locationd/locationd_yawrate.cc +++ b/selfdrive/locationd/locationd_yawrate.cc @@ -12,9 +12,11 @@ void Localizer::update_state(const Eigen::Matrix &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) { - return; + + if (dt < 0) { + 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; diff --git a/selfdrive/locationd/paramsd.cc b/selfdrive/locationd/paramsd.cc index 1766db311b..2d9f11b3eb 100644 --- a/selfdrive/locationd/paramsd.cc +++ b/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(); diff --git a/selfdrive/loggerd/uploader.py b/selfdrive/loggerd/uploader.py index 0bd2ff18f1..59765d13ed 100644 --- a/selfdrive/loggerd/uploader.py +++ b/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: - cloudlog.info("uploader MISSING DONGLE_ID or ACCESS_TOKEN") - raise Exception("uploader can't start without dongle id and access token") + if dongle_id is None or private_key is None: + cloudlog.info("uploader missing dongle_id or private_key") + raise Exception("uploader can't start without dongle id and private key") - uploader = Uploader(dongle_id, access_token, ROOT) + uploader = Uploader(dongle_id, private_key, ROOT) backoff = 0.1 while True: diff --git a/selfdrive/manager.py b/selfdrive/manager.py index 6de644c336..c5990f4fdf 100755 --- a/selfdrive/manager.py +++ b/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'] diff --git a/selfdrive/registration.py b/selfdrive/registration.py index f8a084bd36..5d45b11292 100644 --- a/selfdrive/registration.py +++ b/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() diff --git a/selfdrive/test/openpilotci_upload.py b/selfdrive/test/openpilotci_upload.py new file mode 100755 index 0000000000..85509851de --- /dev/null +++ b/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 + diff --git a/selfdrive/test/plant/plant.py b/selfdrive/test/plant/plant.py index 3cd0d6b167..531ad8ad26 100755 --- a/selfdrive/test/plant/plant.py +++ b/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, diff --git a/selfdrive/test/test_car_models_openpilot.py b/selfdrive/test/test_car_models_openpilot.py new file mode 100755 index 0000000000..53184653c6 --- /dev/null +++ b/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" diff --git a/selfdrive/test/tests/__init__.py b/selfdrive/test/tests/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/selfdrive/test/tests/process_replay/.gitignore b/selfdrive/test/tests/process_replay/.gitignore new file mode 100644 index 0000000000..6d339d54f6 --- /dev/null +++ b/selfdrive/test/tests/process_replay/.gitignore @@ -0,0 +1,2 @@ +*.bz2 +diff.txt diff --git a/selfdrive/test/tests/process_replay/README.md b/selfdrive/test/tests/process_replay/README.md new file mode 100644 index 0000000000..639ca9051d --- /dev/null +++ b/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 + diff --git a/selfdrive/test/tests/process_replay/__init__.py b/selfdrive/test/tests/process_replay/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/selfdrive/test/tests/process_replay/compare_logs.py b/selfdrive/test/tests/process_replay/compare_logs.py new file mode 100755 index 0000000000..260e06c771 --- /dev/null +++ b/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:]) diff --git a/selfdrive/test/tests/process_replay/process_replay.py b/selfdrive/test/tests/process_replay/process_replay.py new file mode 100755 index 0000000000..66067b405d --- /dev/null +++ b/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 + diff --git a/selfdrive/test/tests/process_replay/ref_commit b/selfdrive/test/tests/process_replay/ref_commit new file mode 100644 index 0000000000..30a1a28538 --- /dev/null +++ b/selfdrive/test/tests/process_replay/ref_commit @@ -0,0 +1 @@ +e3388c62ffb80f4b3ca8721da56a581a93c44e79 \ No newline at end of file diff --git a/selfdrive/test/tests/process_replay/test_processes.py b/selfdrive/test/tests/process_replay/test_processes.py new file mode 100755 index 0000000000..8d70c80a2b --- /dev/null +++ b/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)) diff --git a/selfdrive/test/tests/process_replay/update_refs.py b/selfdrive/test/tests/process_replay/update_refs.py new file mode 100755 index 0000000000..4bc2659391 --- /dev/null +++ b/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" diff --git a/selfdrive/ui/slplay.c b/selfdrive/ui/slplay.c index 2085057664..ddfbad56c5 100644 --- a/selfdrive/ui/slplay.c +++ b/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; diff --git a/selfdrive/ui/spinner/spinner b/selfdrive/ui/spinner/spinner index 79f3206a7a..b3997889bd 100755 --- a/selfdrive/ui/spinner/spinner +++ b/selfdrive/ui/spinner/spinner @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:11953df252f1cd63d7cd2101508b89c96f5de073b3181c4262f1a26d3c871ca1 +oid sha256:7a4f45d19c1d85792e4c48a60f7d9beb0264ce187ff5edccf8532fda37fe831d size 239976 diff --git a/selfdrive/ui/spinner/spinner.c b/selfdrive/ui/spinner/spinner.c index ad74251502..3ec36e7403 100644 --- a/selfdrive/ui/spinner/spinner.c +++ b/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); diff --git a/selfdrive/ui/ui.c b/selfdrive/ui/ui.c index 15599414e2..3eedb809c6 100644 --- a/selfdrive/ui/ui.c +++ b/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 }, - { "chimeEngage", "../assets/sounds/engaged.wav", false }, - { "chimeWarning1", "../assets/sounds/warning_1.wav", false }, - { "chimeWarning2", "../assets/sounds/warning_2.wav", false }, - { "chimeWarningRepeat", "../assets/sounds/warning_2.wav", true }, - { "chimeError", "../assets/sounds/error.wav", false }, - { "chimePrompt", "../assets/sounds/error.wav", false }, - { NULL, NULL, false }, + { cereal_CarControl_HUDControl_AudibleAlert_chimeDisengage, "../assets/sounds/disengaged.wav", false }, + { cereal_CarControl_HUDControl_AudibleAlert_chimeEngage, "../assets/sounds/engaged.wav", false }, + { cereal_CarControl_HUDControl_AudibleAlert_chimeWarning1, "../assets/sounds/warning_1.wav", false }, + { cereal_CarControl_HUDControl_AudibleAlert_chimeWarning2, "../assets/sounds/warning_2.wav", false }, + { cereal_CarControl_HUDControl_AudibleAlert_chimeWarningRepeat, "../assets/sounds/warning_2.wav", true }, + { cereal_CarControl_HUDControl_AudibleAlert_chimeError, "../assets/sounds/error.wav", false }, + { cereal_CarControl_HUDControl_AudibleAlert_chimePrompt, "../assets/sounds/error.wav", false }, + { cereal_CarControl_HUDControl_AudibleAlert_none, NULL, false }, }; -sound_file* get_sound_file_by_name(const char* name) { - for (sound_file *s = sound_table; s->name != NULL; s++) { - if (strcmp(s->name, name) == 0) { +sound_file* get_sound_file(AudibleAlert alert) { + for (sound_file *s = sound_table; s->alert != cereal_CarControl_HUDControl_AudibleAlert_none; s++) { + 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 ? 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 ? 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), "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') { - sound_file* active_sound = get_sound_file_by_name(s->alert_sound); + if (s->alert_sound != cereal_CarControl_HUDControl_AudibleAlert_none) { + sound_file* active_sound = get_sound_file(s->alert_sound); slplay_stop_uri(active_sound->uri, &error); if (error) { LOGW("error stopping active sound %s", error); } } - sound_file* sound = get_sound_file_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') { - sound_file* sound = get_sound_file_by_name(s->alert_sound); + } else if ((!datad.alertSound || datad.alertSound == cereal_CarControl_HUDControl_AudibleAlert_none) && s->alert_sound != cereal_CarControl_HUDControl_AudibleAlert_none) { + sound_file* sound = get_sound_file(s->alert_sound); char* error = NULL; @@ -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); diff --git a/selfdrive/visiond/build_from_src.mk b/selfdrive/visiond/build_from_src.mk index d9d320cf3b..41e844fe1d 100644 --- a/selfdrive/visiond/build_from_src.mk +++ b/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_LIBS = -l:libczmq.a -l:libzmq.a -lsodium + ZMQ_FLAGS = -I$(PHONELIBS)/zmq/x64/include + 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 \ diff --git a/selfdrive/visiond/models/driving.cc b/selfdrive/visiond/models/driving.cc index 6559e16475..33b7ec10ff 100644 --- a/selfdrive/visiond/models/driving.cc +++ b/selfdrive/visiond/models/driving.cc @@ -4,36 +4,32 @@ #include #ifdef QCOM - #include +#include #else - #include +#include #endif #include "common/timing.h" #include "driving.h" -#ifdef MEDMODEL - #define MODEL_WIDTH 512 - #define MODEL_HEIGHT 256 - #define MODEL_NAME "driving_model_dlc" -#else - #define MODEL_WIDTH 320 - #define MODEL_HEIGHT 160 - #define MODEL_NAME "driving_model_dlc" -#endif +#define MODEL_WIDTH 512 +#define MODEL_HEIGHT 256 +#define MODEL_NAME "driving_model_dlc" #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 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 net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + 8]) { @@ -127,8 +134,8 @@ ModelData model_eval_frame(ModelState* s, cl_command_queue q, model.lead.rel_v_std = softplus(net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + MDN_VALS + 2]) * max_rel_vel; 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 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; - for (int idx = 0; idx < SPEED_BUCKETS; idx++) { - sum += net_outputs.speed[idx]; - int idx_percentile = (sum + .05) * SPEED_PERCENTILES; - if (idx_percentile < SPEED_PERCENTILES ){ - model.speed[idx_percentile] = ((float)idx)/2.0; - } - } + //float sum = 0; + //for (int idx = 0; idx < SPEED_BUCKETS; idx++) { + // sum += net_outputs.speed[idx]; + // int idx_percentile = (sum + .05) * SPEED_PERCENTILES; + // if (idx_percentile < SPEED_PERCENTILES ){ + // model.speed[idx_percentile] = ((float)idx)/2.0; + // } + //} // make sure no percentiles are skipped - for (int i=SPEED_PERCENTILES-1; i > 0; i--){ - if (model.speed[i-1] > model.speed[i]){ - model.speed[i-1] = model.speed[i]; - } - } + //for (int i=SPEED_PERCENTILES-1; i > 0; i--){ + // if (model.speed[i-1] > model.speed[i]){ + // model.speed[i-1] = model.speed[i]; + // } + //} return model; } diff --git a/selfdrive/visiond/models/driving.h b/selfdrive/visiond/models/driving.h index 2afb205217..966cf69473 100644 --- a/selfdrive/visiond/models/driving.h +++ b/selfdrive/visiond/models/driving.h @@ -2,7 +2,6 @@ #define MODEL_H // gate this here -#define MEDMODEL #define TEMPORAL #include "common/mat.h" diff --git a/selfdrive/visiond/models/monitoring.cc b/selfdrive/visiond/models/monitoring.cc index 9e874aa0e0..f6e47880fd 100644 --- a/selfdrive/visiond/models/monitoring.cc +++ b/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; } diff --git a/selfdrive/visiond/models/monitoring.h b/selfdrive/visiond/models/monitoring.h index 80c65d0f0e..2be96825f8 100644 --- a/selfdrive/visiond/models/monitoring.h +++ b/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 { diff --git a/selfdrive/visiond/runners/run.h b/selfdrive/visiond/runners/run.h index d92519daf1..049f065845 100644 --- a/selfdrive/visiond/runners/run.h +++ b/selfdrive/visiond/runners/run.h @@ -7,8 +7,9 @@ #ifdef QCOM #define DefaultRunModel SNPEModel #else - #include "tfmodel.h" - #define DefaultRunModel TFModel +#define DefaultRunModel SNPEModel + /* #include "tfmodel.h" */ + /* #define DefaultRunModel TFModel */ #endif #endif diff --git a/selfdrive/visiond/visiond.cc b/selfdrive/visiond/visiond.cc index dd19912b3a..a0cce70ab8 100644 --- a/selfdrive/visiond/visiond.cc +++ b/selfdrive/visiond/visiond.cc @@ -26,6 +26,12 @@ #include #include +#ifdef QCOM +#include +#else +#include +#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 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 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(); -#else - auto wm2 = event.getLiveCalibration().getWarpMatrix2(); -#endif - assert(wm2.size() == 3*3); + + auto extrinsic_matrix = event.getLiveCalibration().getExtrinsicMatrix(); + Eigen::Matrix extrinsic_matrix_eigen; + for (int i = 0; i < 4*3; i++){ + extrinsic_matrix_eigen(i / 4, i % 4) = extrinsic_matrix[i]; + } + + auto camera_frame_from_road_frame = eon_intrinsics * extrinsic_matrix_eigen; + Eigen::Matrix 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); }