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 c221675d77..5dd8b8f204 100644
Binary files a/apk/ai.comma.plus.offroad.apk and b/apk/ai.comma.plus.offroad.apk differ
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 857a78e6f5..51886bd334 100755
Binary files a/installer/updater/updater and b/installer/updater/updater differ
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 d06aad6190..79181185f7 100644
Binary files a/models/driving_model.dlc and b/models/driving_model.dlc differ
diff --git a/models/monitoring_model.dlc b/models/monitoring_model.dlc
index 47e71afe2e..f7496fb925 100644
Binary files a/models/monitoring_model.dlc and b/models/monitoring_model.dlc differ
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 a355c27cde..0000000000
Binary files a/selfdrive/assets/Roboto-Bold.ttf and /dev/null differ
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 958e08fd85..655aa31261 100644
Binary files a/selfdrive/assets/sounds/disengaged.wav and b/selfdrive/assets/sounds/disengaged.wav differ
diff --git a/selfdrive/assets/sounds/engaged.wav b/selfdrive/assets/sounds/engaged.wav
index c6c088e01c..b33e8181fa 100644
Binary files a/selfdrive/assets/sounds/engaged.wav and b/selfdrive/assets/sounds/engaged.wav differ
diff --git a/selfdrive/assets/sounds/error.wav b/selfdrive/assets/sounds/error.wav
index 1ff0c540d2..309eaef8ae 100644
Binary files a/selfdrive/assets/sounds/error.wav and b/selfdrive/assets/sounds/error.wav differ
diff --git a/selfdrive/assets/sounds/warning_1.wav b/selfdrive/assets/sounds/warning_1.wav
index 67b8d76fe8..920c11846a 100644
Binary files a/selfdrive/assets/sounds/warning_1.wav and b/selfdrive/assets/sounds/warning_1.wav differ
diff --git a/selfdrive/assets/sounds/warning_2.wav b/selfdrive/assets/sounds/warning_2.wav
index 8e1b1d7d91..f5ed8521dd 100644
Binary files a/selfdrive/assets/sounds/warning_2.wav and b/selfdrive/assets/sounds/warning_2.wav differ
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 d5b5672770..f069b62c06 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
@@ -64,7 +64,7 @@ extern "C"
/** Number of control/estimation intervals. */
#define ACADO_N 20
/** Number of online data values. */
-#define ACADO_NOD 18
+#define ACADO_NOD 17
/** Number of path constraints. */
#define ACADO_NPAC 0
/** Number of control variables. */
@@ -114,11 +114,11 @@ real_t x[ 84 ];
*/
real_t u[ 20 ];
-/** Matrix of size: 21 x 18 (row major format)
+/** Matrix of size: 21 x 17 (row major format)
*
* Matrix containing 21 online data vectors.
*/
-real_t od[ 378 ];
+real_t od[ 357 ];
/** Column vector of size: 100
*
@@ -160,14 +160,14 @@ real_t rhs_aux[ 14 ];
real_t rk_ttt;
-/** Row vector of size: 43 */
-real_t rk_xxx[ 43 ];
+/** Row vector of size: 42 */
+real_t rk_xxx[ 42 ];
/** Matrix of size: 4 x 24 (row major format) */
real_t rk_kkk[ 96 ];
-/** Row vector of size: 43 */
-real_t state[ 43 ];
+/** Row vector of size: 42 */
+real_t state[ 42 ];
/** Column vector of size: 80 */
real_t d[ 80 ];
@@ -184,11 +184,11 @@ real_t evGx[ 320 ];
/** Column vector of size: 80 */
real_t evGu[ 80 ];
-/** Column vector of size: 21 */
-real_t objAuxVar[ 21 ];
+/** Column vector of size: 11 */
+real_t objAuxVar[ 11 ];
-/** Row vector of size: 23 */
-real_t objValueIn[ 23 ];
+/** Row vector of size: 22 */
+real_t objValueIn[ 22 ];
/** Row vector of size: 30 */
real_t objValueOut[ 30 ];
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 b931088991..5df7cb47f5 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
@@ -118,7 +118,6 @@ acadoWorkspace.rk_xxx[38] = rk_eta[38];
acadoWorkspace.rk_xxx[39] = rk_eta[39];
acadoWorkspace.rk_xxx[40] = rk_eta[40];
acadoWorkspace.rk_xxx[41] = rk_eta[41];
-acadoWorkspace.rk_xxx[42] = rk_eta[42];
for (run1 = 0; run1 < 1; ++run1)
{
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 014c19a01a..347e77fa73 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
@@ -43,24 +43,23 @@ acadoWorkspace.state[2] = acadoVariables.x[lRun1 * 4 + 2];
acadoWorkspace.state[3] = acadoVariables.x[lRun1 * 4 + 3];
acadoWorkspace.state[24] = acadoVariables.u[lRun1];
-acadoWorkspace.state[25] = acadoVariables.od[lRun1 * 18];
-acadoWorkspace.state[26] = acadoVariables.od[lRun1 * 18 + 1];
-acadoWorkspace.state[27] = acadoVariables.od[lRun1 * 18 + 2];
-acadoWorkspace.state[28] = acadoVariables.od[lRun1 * 18 + 3];
-acadoWorkspace.state[29] = acadoVariables.od[lRun1 * 18 + 4];
-acadoWorkspace.state[30] = acadoVariables.od[lRun1 * 18 + 5];
-acadoWorkspace.state[31] = acadoVariables.od[lRun1 * 18 + 6];
-acadoWorkspace.state[32] = acadoVariables.od[lRun1 * 18 + 7];
-acadoWorkspace.state[33] = acadoVariables.od[lRun1 * 18 + 8];
-acadoWorkspace.state[34] = acadoVariables.od[lRun1 * 18 + 9];
-acadoWorkspace.state[35] = acadoVariables.od[lRun1 * 18 + 10];
-acadoWorkspace.state[36] = acadoVariables.od[lRun1 * 18 + 11];
-acadoWorkspace.state[37] = acadoVariables.od[lRun1 * 18 + 12];
-acadoWorkspace.state[38] = acadoVariables.od[lRun1 * 18 + 13];
-acadoWorkspace.state[39] = acadoVariables.od[lRun1 * 18 + 14];
-acadoWorkspace.state[40] = acadoVariables.od[lRun1 * 18 + 15];
-acadoWorkspace.state[41] = acadoVariables.od[lRun1 * 18 + 16];
-acadoWorkspace.state[42] = acadoVariables.od[lRun1 * 18 + 17];
+acadoWorkspace.state[25] = acadoVariables.od[lRun1 * 17];
+acadoWorkspace.state[26] = acadoVariables.od[lRun1 * 17 + 1];
+acadoWorkspace.state[27] = acadoVariables.od[lRun1 * 17 + 2];
+acadoWorkspace.state[28] = acadoVariables.od[lRun1 * 17 + 3];
+acadoWorkspace.state[29] = acadoVariables.od[lRun1 * 17 + 4];
+acadoWorkspace.state[30] = acadoVariables.od[lRun1 * 17 + 5];
+acadoWorkspace.state[31] = acadoVariables.od[lRun1 * 17 + 6];
+acadoWorkspace.state[32] = acadoVariables.od[lRun1 * 17 + 7];
+acadoWorkspace.state[33] = acadoVariables.od[lRun1 * 17 + 8];
+acadoWorkspace.state[34] = acadoVariables.od[lRun1 * 17 + 9];
+acadoWorkspace.state[35] = acadoVariables.od[lRun1 * 17 + 10];
+acadoWorkspace.state[36] = acadoVariables.od[lRun1 * 17 + 11];
+acadoWorkspace.state[37] = acadoVariables.od[lRun1 * 17 + 12];
+acadoWorkspace.state[38] = acadoVariables.od[lRun1 * 17 + 13];
+acadoWorkspace.state[39] = acadoVariables.od[lRun1 * 17 + 14];
+acadoWorkspace.state[40] = acadoVariables.od[lRun1 * 17 + 15];
+acadoWorkspace.state[41] = acadoVariables.od[lRun1 * 17 + 16];
ret = acado_integrate(acadoWorkspace.state, 1, lRun1);
@@ -99,51 +98,41 @@ void acado_evaluateLSQ(const real_t* in, real_t* out)
const real_t* xd = in;
const real_t* u = in + 4;
const real_t* od = in + 5;
-/* Vector of auxiliary variables; number of elements: 21. */
+/* Vector of auxiliary variables; number of elements: 11. */
real_t* a = acadoWorkspace.objAuxVar;
/* Compute intermediate quantities: */
-a[0] = (exp(((real_t)(0.0000000000000000e+00)-(((od[14]*((((od[2]*((xd[0]*xd[0])*xd[0]))+(od[3]*(xd[0]*xd[0])))+(od[4]*xd[0]))+od[5]))+(((real_t)(1.0000000000000000e+00)-od[14])*((((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*(((((od[2]*((xd[0]*xd[0])*xd[0]))+(od[3]*(xd[0]*xd[0])))+(od[4]*xd[0]))+od[5])-(od[17]/(real_t)(2.0000000000000000e+00))))+(od[15]*(((((od[6]*((xd[0]*xd[0])*xd[0]))+(od[7]*(xd[0]*xd[0])))+(od[8]*xd[0]))+od[9])+(od[17]/(real_t)(2.0000000000000000e+00))))))/((od[14]+od[15])+(real_t)(1.0000000000000000e-04)))+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*((((od[10]*((xd[0]*xd[0])*xd[0]))+(od[11]*(xd[0]*xd[0])))+(od[12]*xd[0]))+od[13])))+(od[17]/(real_t)(2.0000000000000000e+00)))))-xd[1]))));
-a[1] = (exp((((od[15]*((((od[6]*((xd[0]*xd[0])*xd[0]))+(od[7]*(xd[0]*xd[0])))+(od[8]*xd[0]))+od[9]))+(((real_t)(1.0000000000000000e+00)-od[15])*((((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*(((((od[2]*((xd[0]*xd[0])*xd[0]))+(od[3]*(xd[0]*xd[0])))+(od[4]*xd[0]))+od[5])-(od[17]/(real_t)(2.0000000000000000e+00))))+(od[15]*(((((od[6]*((xd[0]*xd[0])*xd[0]))+(od[7]*(xd[0]*xd[0])))+(od[8]*xd[0]))+od[9])+(od[17]/(real_t)(2.0000000000000000e+00))))))/((od[14]+od[15])+(real_t)(1.0000000000000000e-04)))+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*((((od[10]*((xd[0]*xd[0])*xd[0]))+(od[11]*(xd[0]*xd[0])))+(od[12]*xd[0]))+od[13])))-(od[17]/(real_t)(2.0000000000000000e+00)))))-xd[1])));
-a[2] = (atan(((((((real_t)(3.0000000000000000e+00)*od[2])*xd[0])*xd[0])+(((real_t)(2.0000000000000000e+00)*od[3])*xd[0]))+od[4])));
-a[3] = (atan(((((((real_t)(3.0000000000000000e+00)*od[6])*xd[0])*xd[0])+(((real_t)(2.0000000000000000e+00)*od[7])*xd[0]))+od[8])));
-a[4] = (atan(((((((real_t)(3.0000000000000000e+00)*od[10])*xd[0])*xd[0])+(((real_t)(2.0000000000000000e+00)*od[11])*xd[0]))+od[12])));
-a[5] = ((real_t)(1.0000000000000000e+00)/((od[14]+od[15])+(real_t)(1.0000000000000000e-04)));
-a[6] = ((real_t)(1.0000000000000000e+00)/((od[14]+od[15])+(real_t)(1.0000000000000000e-04)));
-a[7] = (exp(((real_t)(0.0000000000000000e+00)-(((od[14]*((((od[2]*((xd[0]*xd[0])*xd[0]))+(od[3]*(xd[0]*xd[0])))+(od[4]*xd[0]))+od[5]))+(((real_t)(1.0000000000000000e+00)-od[14])*((((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*(((((od[2]*((xd[0]*xd[0])*xd[0]))+(od[3]*(xd[0]*xd[0])))+(od[4]*xd[0]))+od[5])-(od[17]/(real_t)(2.0000000000000000e+00))))+(od[15]*(((((od[6]*((xd[0]*xd[0])*xd[0]))+(od[7]*(xd[0]*xd[0])))+(od[8]*xd[0]))+od[9])+(od[17]/(real_t)(2.0000000000000000e+00))))))/((od[14]+od[15])+(real_t)(1.0000000000000000e-04)))+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*((((od[10]*((xd[0]*xd[0])*xd[0]))+(od[11]*(xd[0]*xd[0])))+(od[12]*xd[0]))+od[13])))+(od[17]/(real_t)(2.0000000000000000e+00)))))-xd[1]))));
-a[8] = (((real_t)(0.0000000000000000e+00)-((od[14]*(((od[2]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[3]*(xd[0]+xd[0])))+od[4]))+(((real_t)(1.0000000000000000e+00)-od[14])*(((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*(((od[2]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[3]*(xd[0]+xd[0])))+od[4]))+(od[15]*(((od[6]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[7]*(xd[0]+xd[0])))+od[8]))))*a[6])+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*(((od[10]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[11]*(xd[0]+xd[0])))+od[12]))))))*a[7]);
-a[9] = (((real_t)(0.0000000000000000e+00)-((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00)))*a[7]);
-a[10] = ((real_t)(1.0000000000000000e+00)/((od[14]+od[15])+(real_t)(1.0000000000000000e-04)));
-a[11] = (exp((((od[15]*((((od[6]*((xd[0]*xd[0])*xd[0]))+(od[7]*(xd[0]*xd[0])))+(od[8]*xd[0]))+od[9]))+(((real_t)(1.0000000000000000e+00)-od[15])*((((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*(((((od[2]*((xd[0]*xd[0])*xd[0]))+(od[3]*(xd[0]*xd[0])))+(od[4]*xd[0]))+od[5])-(od[17]/(real_t)(2.0000000000000000e+00))))+(od[15]*(((((od[6]*((xd[0]*xd[0])*xd[0]))+(od[7]*(xd[0]*xd[0])))+(od[8]*xd[0]))+od[9])+(od[17]/(real_t)(2.0000000000000000e+00))))))/((od[14]+od[15])+(real_t)(1.0000000000000000e-04)))+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*((((od[10]*((xd[0]*xd[0])*xd[0]))+(od[11]*(xd[0]*xd[0])))+(od[12]*xd[0]))+od[13])))-(od[17]/(real_t)(2.0000000000000000e+00)))))-xd[1])));
-a[12] = (((od[15]*(((od[6]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[7]*(xd[0]+xd[0])))+od[8]))+(((real_t)(1.0000000000000000e+00)-od[15])*(((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*(((od[2]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[3]*(xd[0]+xd[0])))+od[4]))+(od[15]*(((od[6]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[7]*(xd[0]+xd[0])))+od[8]))))*a[10])+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*(((od[10]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[11]*(xd[0]+xd[0])))+od[12])))))*a[11]);
-a[13] = (((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00))*a[11]);
-a[14] = ((real_t)(1.0000000000000000e+00)/((real_t)(1.0000000000000000e+00)+(pow(((((((real_t)(3.0000000000000000e+00)*od[2])*xd[0])*xd[0])+(((real_t)(2.0000000000000000e+00)*od[3])*xd[0]))+od[4]),2))));
-a[15] = ((((((real_t)(3.0000000000000000e+00)*od[2])*xd[0])+(((real_t)(3.0000000000000000e+00)*od[2])*xd[0]))+((real_t)(2.0000000000000000e+00)*od[3]))*a[14]);
-a[16] = ((real_t)(1.0000000000000000e+00)/((real_t)(1.0000000000000000e+00)+(pow(((((((real_t)(3.0000000000000000e+00)*od[6])*xd[0])*xd[0])+(((real_t)(2.0000000000000000e+00)*od[7])*xd[0]))+od[8]),2))));
-a[17] = ((((((real_t)(3.0000000000000000e+00)*od[6])*xd[0])+(((real_t)(3.0000000000000000e+00)*od[6])*xd[0]))+((real_t)(2.0000000000000000e+00)*od[7]))*a[16]);
-a[18] = ((real_t)(1.0000000000000000e+00)/((od[14]+od[15])+(real_t)(1.0000000000000000e-04)));
-a[19] = ((real_t)(1.0000000000000000e+00)/((real_t)(1.0000000000000000e+00)+(pow(((((((real_t)(3.0000000000000000e+00)*od[10])*xd[0])*xd[0])+(((real_t)(2.0000000000000000e+00)*od[11])*xd[0]))+od[12]),2))));
-a[20] = ((((((real_t)(3.0000000000000000e+00)*od[10])*xd[0])+(((real_t)(3.0000000000000000e+00)*od[10])*xd[0]))+((real_t)(2.0000000000000000e+00)*od[11]))*a[19]);
+a[0] = (exp(((real_t)(0.0000000000000000e+00)-(((od[14]*((((od[2]*((xd[0]*xd[0])*xd[0]))+(od[3]*(xd[0]*xd[0])))+(od[4]*xd[0]))+od[5]))+(((real_t)(1.0000000000000000e+00)-od[14])*(((((od[10]*((xd[0]*xd[0])*xd[0]))+(od[11]*(xd[0]*xd[0])))+(od[12]*xd[0]))+od[13])+(od[16]/(real_t)(2.0000000000000000e+00)))))-xd[1]))));
+a[1] = (exp((((od[15]*((((od[6]*((xd[0]*xd[0])*xd[0]))+(od[7]*(xd[0]*xd[0])))+(od[8]*xd[0]))+od[9]))+(((real_t)(1.0000000000000000e+00)-od[15])*(((((od[10]*((xd[0]*xd[0])*xd[0]))+(od[11]*(xd[0]*xd[0])))+(od[12]*xd[0]))+od[13])-(od[16]/(real_t)(2.0000000000000000e+00)))))-xd[1])));
+a[2] = (atan(((((((real_t)(3.0000000000000000e+00)*od[10])*xd[0])*xd[0])+(((real_t)(2.0000000000000000e+00)*od[11])*xd[0]))+od[12])));
+a[3] = (exp(((real_t)(0.0000000000000000e+00)-(((od[14]*((((od[2]*((xd[0]*xd[0])*xd[0]))+(od[3]*(xd[0]*xd[0])))+(od[4]*xd[0]))+od[5]))+(((real_t)(1.0000000000000000e+00)-od[14])*(((((od[10]*((xd[0]*xd[0])*xd[0]))+(od[11]*(xd[0]*xd[0])))+(od[12]*xd[0]))+od[13])+(od[16]/(real_t)(2.0000000000000000e+00)))))-xd[1]))));
+a[4] = (((real_t)(0.0000000000000000e+00)-((od[14]*(((od[2]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[3]*(xd[0]+xd[0])))+od[4]))+(((real_t)(1.0000000000000000e+00)-od[14])*(((od[10]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[11]*(xd[0]+xd[0])))+od[12]))))*a[3]);
+a[5] = (((real_t)(0.0000000000000000e+00)-((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00)))*a[3]);
+a[6] = (exp((((od[15]*((((od[6]*((xd[0]*xd[0])*xd[0]))+(od[7]*(xd[0]*xd[0])))+(od[8]*xd[0]))+od[9]))+(((real_t)(1.0000000000000000e+00)-od[15])*(((((od[10]*((xd[0]*xd[0])*xd[0]))+(od[11]*(xd[0]*xd[0])))+(od[12]*xd[0]))+od[13])-(od[16]/(real_t)(2.0000000000000000e+00)))))-xd[1])));
+a[7] = (((od[15]*(((od[6]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[7]*(xd[0]+xd[0])))+od[8]))+(((real_t)(1.0000000000000000e+00)-od[15])*(((od[10]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[11]*(xd[0]+xd[0])))+od[12])))*a[6]);
+a[8] = (((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00))*a[6]);
+a[9] = ((real_t)(1.0000000000000000e+00)/((real_t)(1.0000000000000000e+00)+(pow(((((((real_t)(3.0000000000000000e+00)*od[10])*xd[0])*xd[0])+(((real_t)(2.0000000000000000e+00)*od[11])*xd[0]))+od[12]),2))));
+a[10] = ((((((real_t)(3.0000000000000000e+00)*od[10])*xd[0])+(((real_t)(3.0000000000000000e+00)*od[10])*xd[0]))+((real_t)(2.0000000000000000e+00)*od[11]))*a[9]);
/* Compute outputs: */
-out[0] = ((((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*(((((od[2]*((xd[0]*xd[0])*xd[0]))+(od[3]*(xd[0]*xd[0])))+(od[4]*xd[0]))+od[5])-(od[17]/(real_t)(2.0000000000000000e+00))))+(od[15]*(((((od[6]*((xd[0]*xd[0])*xd[0]))+(od[7]*(xd[0]*xd[0])))+(od[8]*xd[0]))+od[9])+(od[17]/(real_t)(2.0000000000000000e+00))))))/((od[14]+od[15])+(real_t)(1.0000000000000000e-04)))+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*((((od[10]*((xd[0]*xd[0])*xd[0]))+(od[11]*(xd[0]*xd[0])))+(od[12]*xd[0]))+od[13])))-xd[1]);
+out[0] = (((((od[10]*((xd[0]*xd[0])*xd[0]))+(od[11]*(xd[0]*xd[0])))+(od[12]*xd[0]))+od[13])-xd[1]);
out[1] = (((od[14]+od[15])-(od[14]*od[15]))*a[0]);
out[2] = (((od[14]+od[15])-(od[14]*od[15]))*a[1]);
-out[3] = ((od[1]+(real_t)(1.0000000000000000e+00))*((((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*a[2])+(od[15]*a[3])))/((od[14]+od[15])+(real_t)(1.0000000000000000e-04)))+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*a[4]))-xd[2]));
+out[3] = ((od[1]+(real_t)(1.0000000000000000e+00))*(a[2]-xd[2]));
out[4] = ((od[1]+(real_t)(1.0000000000000000e+00))*u[0]);
-out[5] = (((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*(((od[2]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[3]*(xd[0]+xd[0])))+od[4]))+(od[15]*(((od[6]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[7]*(xd[0]+xd[0])))+od[8]))))*a[5])+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*(((od[10]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[11]*(xd[0]+xd[0])))+od[12])));
+out[5] = (((od[10]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[11]*(xd[0]+xd[0])))+od[12]);
out[6] = ((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00));
out[7] = (real_t)(0.0000000000000000e+00);
out[8] = (real_t)(0.0000000000000000e+00);
-out[9] = (((od[14]+od[15])-(od[14]*od[15]))*a[8]);
-out[10] = (((od[14]+od[15])-(od[14]*od[15]))*a[9]);
+out[9] = (((od[14]+od[15])-(od[14]*od[15]))*a[4]);
+out[10] = (((od[14]+od[15])-(od[14]*od[15]))*a[5]);
out[11] = (real_t)(0.0000000000000000e+00);
out[12] = (real_t)(0.0000000000000000e+00);
-out[13] = (((od[14]+od[15])-(od[14]*od[15]))*a[12]);
-out[14] = (((od[14]+od[15])-(od[14]*od[15]))*a[13]);
+out[13] = (((od[14]+od[15])-(od[14]*od[15]))*a[7]);
+out[14] = (((od[14]+od[15])-(od[14]*od[15]))*a[8]);
out[15] = (real_t)(0.0000000000000000e+00);
out[16] = (real_t)(0.0000000000000000e+00);
-out[17] = ((od[1]+(real_t)(1.0000000000000000e+00))*(((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*a[15])+(od[15]*a[17])))*a[18])+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*a[20])));
+out[17] = ((od[1]+(real_t)(1.0000000000000000e+00))*a[10]);
out[18] = (real_t)(0.0000000000000000e+00);
out[19] = ((od[1]+(real_t)(1.0000000000000000e+00))*((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00)));
out[20] = (real_t)(0.0000000000000000e+00);
@@ -162,50 +151,40 @@ void acado_evaluateLSQEndTerm(const real_t* in, real_t* out)
{
const real_t* xd = in;
const real_t* od = in + 4;
-/* Vector of auxiliary variables; number of elements: 21. */
+/* Vector of auxiliary variables; number of elements: 11. */
real_t* a = acadoWorkspace.objAuxVar;
/* Compute intermediate quantities: */
-a[0] = (exp(((real_t)(0.0000000000000000e+00)-(((od[14]*((((od[2]*((xd[0]*xd[0])*xd[0]))+(od[3]*(xd[0]*xd[0])))+(od[4]*xd[0]))+od[5]))+(((real_t)(1.0000000000000000e+00)-od[14])*((((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*(((((od[2]*((xd[0]*xd[0])*xd[0]))+(od[3]*(xd[0]*xd[0])))+(od[4]*xd[0]))+od[5])-(od[17]/(real_t)(2.0000000000000000e+00))))+(od[15]*(((((od[6]*((xd[0]*xd[0])*xd[0]))+(od[7]*(xd[0]*xd[0])))+(od[8]*xd[0]))+od[9])+(od[17]/(real_t)(2.0000000000000000e+00))))))/((od[14]+od[15])+(real_t)(1.0000000000000000e-04)))+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*((((od[10]*((xd[0]*xd[0])*xd[0]))+(od[11]*(xd[0]*xd[0])))+(od[12]*xd[0]))+od[13])))+(od[17]/(real_t)(2.0000000000000000e+00)))))-xd[1]))));
-a[1] = (exp((((od[15]*((((od[6]*((xd[0]*xd[0])*xd[0]))+(od[7]*(xd[0]*xd[0])))+(od[8]*xd[0]))+od[9]))+(((real_t)(1.0000000000000000e+00)-od[15])*((((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*(((((od[2]*((xd[0]*xd[0])*xd[0]))+(od[3]*(xd[0]*xd[0])))+(od[4]*xd[0]))+od[5])-(od[17]/(real_t)(2.0000000000000000e+00))))+(od[15]*(((((od[6]*((xd[0]*xd[0])*xd[0]))+(od[7]*(xd[0]*xd[0])))+(od[8]*xd[0]))+od[9])+(od[17]/(real_t)(2.0000000000000000e+00))))))/((od[14]+od[15])+(real_t)(1.0000000000000000e-04)))+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*((((od[10]*((xd[0]*xd[0])*xd[0]))+(od[11]*(xd[0]*xd[0])))+(od[12]*xd[0]))+od[13])))-(od[17]/(real_t)(2.0000000000000000e+00)))))-xd[1])));
-a[2] = (atan(((((((real_t)(3.0000000000000000e+00)*od[2])*xd[0])*xd[0])+(((real_t)(2.0000000000000000e+00)*od[3])*xd[0]))+od[4])));
-a[3] = (atan(((((((real_t)(3.0000000000000000e+00)*od[6])*xd[0])*xd[0])+(((real_t)(2.0000000000000000e+00)*od[7])*xd[0]))+od[8])));
-a[4] = (atan(((((((real_t)(3.0000000000000000e+00)*od[10])*xd[0])*xd[0])+(((real_t)(2.0000000000000000e+00)*od[11])*xd[0]))+od[12])));
-a[5] = ((real_t)(1.0000000000000000e+00)/((od[14]+od[15])+(real_t)(1.0000000000000000e-04)));
-a[6] = ((real_t)(1.0000000000000000e+00)/((od[14]+od[15])+(real_t)(1.0000000000000000e-04)));
-a[7] = (exp(((real_t)(0.0000000000000000e+00)-(((od[14]*((((od[2]*((xd[0]*xd[0])*xd[0]))+(od[3]*(xd[0]*xd[0])))+(od[4]*xd[0]))+od[5]))+(((real_t)(1.0000000000000000e+00)-od[14])*((((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*(((((od[2]*((xd[0]*xd[0])*xd[0]))+(od[3]*(xd[0]*xd[0])))+(od[4]*xd[0]))+od[5])-(od[17]/(real_t)(2.0000000000000000e+00))))+(od[15]*(((((od[6]*((xd[0]*xd[0])*xd[0]))+(od[7]*(xd[0]*xd[0])))+(od[8]*xd[0]))+od[9])+(od[17]/(real_t)(2.0000000000000000e+00))))))/((od[14]+od[15])+(real_t)(1.0000000000000000e-04)))+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*((((od[10]*((xd[0]*xd[0])*xd[0]))+(od[11]*(xd[0]*xd[0])))+(od[12]*xd[0]))+od[13])))+(od[17]/(real_t)(2.0000000000000000e+00)))))-xd[1]))));
-a[8] = (((real_t)(0.0000000000000000e+00)-((od[14]*(((od[2]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[3]*(xd[0]+xd[0])))+od[4]))+(((real_t)(1.0000000000000000e+00)-od[14])*(((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*(((od[2]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[3]*(xd[0]+xd[0])))+od[4]))+(od[15]*(((od[6]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[7]*(xd[0]+xd[0])))+od[8]))))*a[6])+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*(((od[10]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[11]*(xd[0]+xd[0])))+od[12]))))))*a[7]);
-a[9] = (((real_t)(0.0000000000000000e+00)-((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00)))*a[7]);
-a[10] = ((real_t)(1.0000000000000000e+00)/((od[14]+od[15])+(real_t)(1.0000000000000000e-04)));
-a[11] = (exp((((od[15]*((((od[6]*((xd[0]*xd[0])*xd[0]))+(od[7]*(xd[0]*xd[0])))+(od[8]*xd[0]))+od[9]))+(((real_t)(1.0000000000000000e+00)-od[15])*((((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*(((((od[2]*((xd[0]*xd[0])*xd[0]))+(od[3]*(xd[0]*xd[0])))+(od[4]*xd[0]))+od[5])-(od[17]/(real_t)(2.0000000000000000e+00))))+(od[15]*(((((od[6]*((xd[0]*xd[0])*xd[0]))+(od[7]*(xd[0]*xd[0])))+(od[8]*xd[0]))+od[9])+(od[17]/(real_t)(2.0000000000000000e+00))))))/((od[14]+od[15])+(real_t)(1.0000000000000000e-04)))+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*((((od[10]*((xd[0]*xd[0])*xd[0]))+(od[11]*(xd[0]*xd[0])))+(od[12]*xd[0]))+od[13])))-(od[17]/(real_t)(2.0000000000000000e+00)))))-xd[1])));
-a[12] = (((od[15]*(((od[6]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[7]*(xd[0]+xd[0])))+od[8]))+(((real_t)(1.0000000000000000e+00)-od[15])*(((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*(((od[2]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[3]*(xd[0]+xd[0])))+od[4]))+(od[15]*(((od[6]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[7]*(xd[0]+xd[0])))+od[8]))))*a[10])+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*(((od[10]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[11]*(xd[0]+xd[0])))+od[12])))))*a[11]);
-a[13] = (((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00))*a[11]);
-a[14] = ((real_t)(1.0000000000000000e+00)/((real_t)(1.0000000000000000e+00)+(pow(((((((real_t)(3.0000000000000000e+00)*od[2])*xd[0])*xd[0])+(((real_t)(2.0000000000000000e+00)*od[3])*xd[0]))+od[4]),2))));
-a[15] = ((((((real_t)(3.0000000000000000e+00)*od[2])*xd[0])+(((real_t)(3.0000000000000000e+00)*od[2])*xd[0]))+((real_t)(2.0000000000000000e+00)*od[3]))*a[14]);
-a[16] = ((real_t)(1.0000000000000000e+00)/((real_t)(1.0000000000000000e+00)+(pow(((((((real_t)(3.0000000000000000e+00)*od[6])*xd[0])*xd[0])+(((real_t)(2.0000000000000000e+00)*od[7])*xd[0]))+od[8]),2))));
-a[17] = ((((((real_t)(3.0000000000000000e+00)*od[6])*xd[0])+(((real_t)(3.0000000000000000e+00)*od[6])*xd[0]))+((real_t)(2.0000000000000000e+00)*od[7]))*a[16]);
-a[18] = ((real_t)(1.0000000000000000e+00)/((od[14]+od[15])+(real_t)(1.0000000000000000e-04)));
-a[19] = ((real_t)(1.0000000000000000e+00)/((real_t)(1.0000000000000000e+00)+(pow(((((((real_t)(3.0000000000000000e+00)*od[10])*xd[0])*xd[0])+(((real_t)(2.0000000000000000e+00)*od[11])*xd[0]))+od[12]),2))));
-a[20] = ((((((real_t)(3.0000000000000000e+00)*od[10])*xd[0])+(((real_t)(3.0000000000000000e+00)*od[10])*xd[0]))+((real_t)(2.0000000000000000e+00)*od[11]))*a[19]);
+a[0] = (exp(((real_t)(0.0000000000000000e+00)-(((od[14]*((((od[2]*((xd[0]*xd[0])*xd[0]))+(od[3]*(xd[0]*xd[0])))+(od[4]*xd[0]))+od[5]))+(((real_t)(1.0000000000000000e+00)-od[14])*(((((od[10]*((xd[0]*xd[0])*xd[0]))+(od[11]*(xd[0]*xd[0])))+(od[12]*xd[0]))+od[13])+(od[16]/(real_t)(2.0000000000000000e+00)))))-xd[1]))));
+a[1] = (exp((((od[15]*((((od[6]*((xd[0]*xd[0])*xd[0]))+(od[7]*(xd[0]*xd[0])))+(od[8]*xd[0]))+od[9]))+(((real_t)(1.0000000000000000e+00)-od[15])*(((((od[10]*((xd[0]*xd[0])*xd[0]))+(od[11]*(xd[0]*xd[0])))+(od[12]*xd[0]))+od[13])-(od[16]/(real_t)(2.0000000000000000e+00)))))-xd[1])));
+a[2] = (atan(((((((real_t)(3.0000000000000000e+00)*od[10])*xd[0])*xd[0])+(((real_t)(2.0000000000000000e+00)*od[11])*xd[0]))+od[12])));
+a[3] = (exp(((real_t)(0.0000000000000000e+00)-(((od[14]*((((od[2]*((xd[0]*xd[0])*xd[0]))+(od[3]*(xd[0]*xd[0])))+(od[4]*xd[0]))+od[5]))+(((real_t)(1.0000000000000000e+00)-od[14])*(((((od[10]*((xd[0]*xd[0])*xd[0]))+(od[11]*(xd[0]*xd[0])))+(od[12]*xd[0]))+od[13])+(od[16]/(real_t)(2.0000000000000000e+00)))))-xd[1]))));
+a[4] = (((real_t)(0.0000000000000000e+00)-((od[14]*(((od[2]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[3]*(xd[0]+xd[0])))+od[4]))+(((real_t)(1.0000000000000000e+00)-od[14])*(((od[10]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[11]*(xd[0]+xd[0])))+od[12]))))*a[3]);
+a[5] = (((real_t)(0.0000000000000000e+00)-((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00)))*a[3]);
+a[6] = (exp((((od[15]*((((od[6]*((xd[0]*xd[0])*xd[0]))+(od[7]*(xd[0]*xd[0])))+(od[8]*xd[0]))+od[9]))+(((real_t)(1.0000000000000000e+00)-od[15])*(((((od[10]*((xd[0]*xd[0])*xd[0]))+(od[11]*(xd[0]*xd[0])))+(od[12]*xd[0]))+od[13])-(od[16]/(real_t)(2.0000000000000000e+00)))))-xd[1])));
+a[7] = (((od[15]*(((od[6]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[7]*(xd[0]+xd[0])))+od[8]))+(((real_t)(1.0000000000000000e+00)-od[15])*(((od[10]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[11]*(xd[0]+xd[0])))+od[12])))*a[6]);
+a[8] = (((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00))*a[6]);
+a[9] = ((real_t)(1.0000000000000000e+00)/((real_t)(1.0000000000000000e+00)+(pow(((((((real_t)(3.0000000000000000e+00)*od[10])*xd[0])*xd[0])+(((real_t)(2.0000000000000000e+00)*od[11])*xd[0]))+od[12]),2))));
+a[10] = ((((((real_t)(3.0000000000000000e+00)*od[10])*xd[0])+(((real_t)(3.0000000000000000e+00)*od[10])*xd[0]))+((real_t)(2.0000000000000000e+00)*od[11]))*a[9]);
/* Compute outputs: */
-out[0] = ((((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*(((((od[2]*((xd[0]*xd[0])*xd[0]))+(od[3]*(xd[0]*xd[0])))+(od[4]*xd[0]))+od[5])-(od[17]/(real_t)(2.0000000000000000e+00))))+(od[15]*(((((od[6]*((xd[0]*xd[0])*xd[0]))+(od[7]*(xd[0]*xd[0])))+(od[8]*xd[0]))+od[9])+(od[17]/(real_t)(2.0000000000000000e+00))))))/((od[14]+od[15])+(real_t)(1.0000000000000000e-04)))+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*((((od[10]*((xd[0]*xd[0])*xd[0]))+(od[11]*(xd[0]*xd[0])))+(od[12]*xd[0]))+od[13])))-xd[1]);
+out[0] = (((((od[10]*((xd[0]*xd[0])*xd[0]))+(od[11]*(xd[0]*xd[0])))+(od[12]*xd[0]))+od[13])-xd[1]);
out[1] = (od[14]*a[0]);
out[2] = (od[15]*a[1]);
-out[3] = ((((real_t)(2.0000000000000000e+00)*od[1])+(real_t)(1.0000000000000000e+00))*((((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*a[2])+(od[15]*a[3])))/((od[14]+od[15])+(real_t)(1.0000000000000000e-04)))+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*a[4]))-xd[2]));
-out[4] = (((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*(((od[2]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[3]*(xd[0]+xd[0])))+od[4]))+(od[15]*(((od[6]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[7]*(xd[0]+xd[0])))+od[8]))))*a[5])+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*(((od[10]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[11]*(xd[0]+xd[0])))+od[12])));
+out[3] = ((((real_t)(2.0000000000000000e+00)*od[1])+(real_t)(1.0000000000000000e+00))*(a[2]-xd[2]));
+out[4] = (((od[10]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[11]*(xd[0]+xd[0])))+od[12]);
out[5] = ((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00));
out[6] = (real_t)(0.0000000000000000e+00);
out[7] = (real_t)(0.0000000000000000e+00);
-out[8] = (od[14]*a[8]);
-out[9] = (od[14]*a[9]);
+out[8] = (od[14]*a[4]);
+out[9] = (od[14]*a[5]);
out[10] = (real_t)(0.0000000000000000e+00);
out[11] = (real_t)(0.0000000000000000e+00);
-out[12] = (od[15]*a[12]);
-out[13] = (od[15]*a[13]);
+out[12] = (od[15]*a[7]);
+out[13] = (od[15]*a[8]);
out[14] = (real_t)(0.0000000000000000e+00);
out[15] = (real_t)(0.0000000000000000e+00);
-out[16] = ((((real_t)(2.0000000000000000e+00)*od[1])+(real_t)(1.0000000000000000e+00))*(((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*a[15])+(od[15]*a[17])))*a[18])+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*a[20])));
+out[16] = ((((real_t)(2.0000000000000000e+00)*od[1])+(real_t)(1.0000000000000000e+00))*a[10]);
out[17] = (real_t)(0.0000000000000000e+00);
out[18] = ((((real_t)(2.0000000000000000e+00)*od[1])+(real_t)(1.0000000000000000e+00))*((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00)));
out[19] = (real_t)(0.0000000000000000e+00);
@@ -307,24 +286,23 @@ acadoWorkspace.objValueIn[1] = acadoVariables.x[runObj * 4 + 1];
acadoWorkspace.objValueIn[2] = acadoVariables.x[runObj * 4 + 2];
acadoWorkspace.objValueIn[3] = acadoVariables.x[runObj * 4 + 3];
acadoWorkspace.objValueIn[4] = acadoVariables.u[runObj];
-acadoWorkspace.objValueIn[5] = acadoVariables.od[runObj * 18];
-acadoWorkspace.objValueIn[6] = acadoVariables.od[runObj * 18 + 1];
-acadoWorkspace.objValueIn[7] = acadoVariables.od[runObj * 18 + 2];
-acadoWorkspace.objValueIn[8] = acadoVariables.od[runObj * 18 + 3];
-acadoWorkspace.objValueIn[9] = acadoVariables.od[runObj * 18 + 4];
-acadoWorkspace.objValueIn[10] = acadoVariables.od[runObj * 18 + 5];
-acadoWorkspace.objValueIn[11] = acadoVariables.od[runObj * 18 + 6];
-acadoWorkspace.objValueIn[12] = acadoVariables.od[runObj * 18 + 7];
-acadoWorkspace.objValueIn[13] = acadoVariables.od[runObj * 18 + 8];
-acadoWorkspace.objValueIn[14] = acadoVariables.od[runObj * 18 + 9];
-acadoWorkspace.objValueIn[15] = acadoVariables.od[runObj * 18 + 10];
-acadoWorkspace.objValueIn[16] = acadoVariables.od[runObj * 18 + 11];
-acadoWorkspace.objValueIn[17] = acadoVariables.od[runObj * 18 + 12];
-acadoWorkspace.objValueIn[18] = acadoVariables.od[runObj * 18 + 13];
-acadoWorkspace.objValueIn[19] = acadoVariables.od[runObj * 18 + 14];
-acadoWorkspace.objValueIn[20] = acadoVariables.od[runObj * 18 + 15];
-acadoWorkspace.objValueIn[21] = acadoVariables.od[runObj * 18 + 16];
-acadoWorkspace.objValueIn[22] = acadoVariables.od[runObj * 18 + 17];
+acadoWorkspace.objValueIn[5] = acadoVariables.od[runObj * 17];
+acadoWorkspace.objValueIn[6] = acadoVariables.od[runObj * 17 + 1];
+acadoWorkspace.objValueIn[7] = acadoVariables.od[runObj * 17 + 2];
+acadoWorkspace.objValueIn[8] = acadoVariables.od[runObj * 17 + 3];
+acadoWorkspace.objValueIn[9] = acadoVariables.od[runObj * 17 + 4];
+acadoWorkspace.objValueIn[10] = acadoVariables.od[runObj * 17 + 5];
+acadoWorkspace.objValueIn[11] = acadoVariables.od[runObj * 17 + 6];
+acadoWorkspace.objValueIn[12] = acadoVariables.od[runObj * 17 + 7];
+acadoWorkspace.objValueIn[13] = acadoVariables.od[runObj * 17 + 8];
+acadoWorkspace.objValueIn[14] = acadoVariables.od[runObj * 17 + 9];
+acadoWorkspace.objValueIn[15] = acadoVariables.od[runObj * 17 + 10];
+acadoWorkspace.objValueIn[16] = acadoVariables.od[runObj * 17 + 11];
+acadoWorkspace.objValueIn[17] = acadoVariables.od[runObj * 17 + 12];
+acadoWorkspace.objValueIn[18] = acadoVariables.od[runObj * 17 + 13];
+acadoWorkspace.objValueIn[19] = acadoVariables.od[runObj * 17 + 14];
+acadoWorkspace.objValueIn[20] = acadoVariables.od[runObj * 17 + 15];
+acadoWorkspace.objValueIn[21] = acadoVariables.od[runObj * 17 + 16];
acado_evaluateLSQ( acadoWorkspace.objValueIn, acadoWorkspace.objValueOut );
acadoWorkspace.Dy[runObj * 5] = acadoWorkspace.objValueOut[0];
@@ -342,24 +320,23 @@ acadoWorkspace.objValueIn[0] = acadoVariables.x[80];
acadoWorkspace.objValueIn[1] = acadoVariables.x[81];
acadoWorkspace.objValueIn[2] = acadoVariables.x[82];
acadoWorkspace.objValueIn[3] = acadoVariables.x[83];
-acadoWorkspace.objValueIn[4] = acadoVariables.od[360];
-acadoWorkspace.objValueIn[5] = acadoVariables.od[361];
-acadoWorkspace.objValueIn[6] = acadoVariables.od[362];
-acadoWorkspace.objValueIn[7] = acadoVariables.od[363];
-acadoWorkspace.objValueIn[8] = acadoVariables.od[364];
-acadoWorkspace.objValueIn[9] = acadoVariables.od[365];
-acadoWorkspace.objValueIn[10] = acadoVariables.od[366];
-acadoWorkspace.objValueIn[11] = acadoVariables.od[367];
-acadoWorkspace.objValueIn[12] = acadoVariables.od[368];
-acadoWorkspace.objValueIn[13] = acadoVariables.od[369];
-acadoWorkspace.objValueIn[14] = acadoVariables.od[370];
-acadoWorkspace.objValueIn[15] = acadoVariables.od[371];
-acadoWorkspace.objValueIn[16] = acadoVariables.od[372];
-acadoWorkspace.objValueIn[17] = acadoVariables.od[373];
-acadoWorkspace.objValueIn[18] = acadoVariables.od[374];
-acadoWorkspace.objValueIn[19] = acadoVariables.od[375];
-acadoWorkspace.objValueIn[20] = acadoVariables.od[376];
-acadoWorkspace.objValueIn[21] = acadoVariables.od[377];
+acadoWorkspace.objValueIn[4] = acadoVariables.od[340];
+acadoWorkspace.objValueIn[5] = acadoVariables.od[341];
+acadoWorkspace.objValueIn[6] = acadoVariables.od[342];
+acadoWorkspace.objValueIn[7] = acadoVariables.od[343];
+acadoWorkspace.objValueIn[8] = acadoVariables.od[344];
+acadoWorkspace.objValueIn[9] = acadoVariables.od[345];
+acadoWorkspace.objValueIn[10] = acadoVariables.od[346];
+acadoWorkspace.objValueIn[11] = acadoVariables.od[347];
+acadoWorkspace.objValueIn[12] = acadoVariables.od[348];
+acadoWorkspace.objValueIn[13] = acadoVariables.od[349];
+acadoWorkspace.objValueIn[14] = acadoVariables.od[350];
+acadoWorkspace.objValueIn[15] = acadoVariables.od[351];
+acadoWorkspace.objValueIn[16] = acadoVariables.od[352];
+acadoWorkspace.objValueIn[17] = acadoVariables.od[353];
+acadoWorkspace.objValueIn[18] = acadoVariables.od[354];
+acadoWorkspace.objValueIn[19] = acadoVariables.od[355];
+acadoWorkspace.objValueIn[20] = acadoVariables.od[356];
acado_evaluateLSQEndTerm( acadoWorkspace.objValueIn, acadoWorkspace.objValueOut );
acadoWorkspace.DyN[0] = acadoWorkspace.objValueOut[0];
@@ -4966,24 +4943,23 @@ acadoWorkspace.state[1] = acadoVariables.x[index * 4 + 1];
acadoWorkspace.state[2] = acadoVariables.x[index * 4 + 2];
acadoWorkspace.state[3] = acadoVariables.x[index * 4 + 3];
acadoWorkspace.state[24] = acadoVariables.u[index];
-acadoWorkspace.state[25] = acadoVariables.od[index * 18];
-acadoWorkspace.state[26] = acadoVariables.od[index * 18 + 1];
-acadoWorkspace.state[27] = acadoVariables.od[index * 18 + 2];
-acadoWorkspace.state[28] = acadoVariables.od[index * 18 + 3];
-acadoWorkspace.state[29] = acadoVariables.od[index * 18 + 4];
-acadoWorkspace.state[30] = acadoVariables.od[index * 18 + 5];
-acadoWorkspace.state[31] = acadoVariables.od[index * 18 + 6];
-acadoWorkspace.state[32] = acadoVariables.od[index * 18 + 7];
-acadoWorkspace.state[33] = acadoVariables.od[index * 18 + 8];
-acadoWorkspace.state[34] = acadoVariables.od[index * 18 + 9];
-acadoWorkspace.state[35] = acadoVariables.od[index * 18 + 10];
-acadoWorkspace.state[36] = acadoVariables.od[index * 18 + 11];
-acadoWorkspace.state[37] = acadoVariables.od[index * 18 + 12];
-acadoWorkspace.state[38] = acadoVariables.od[index * 18 + 13];
-acadoWorkspace.state[39] = acadoVariables.od[index * 18 + 14];
-acadoWorkspace.state[40] = acadoVariables.od[index * 18 + 15];
-acadoWorkspace.state[41] = acadoVariables.od[index * 18 + 16];
-acadoWorkspace.state[42] = acadoVariables.od[index * 18 + 17];
+acadoWorkspace.state[25] = acadoVariables.od[index * 17];
+acadoWorkspace.state[26] = acadoVariables.od[index * 17 + 1];
+acadoWorkspace.state[27] = acadoVariables.od[index * 17 + 2];
+acadoWorkspace.state[28] = acadoVariables.od[index * 17 + 3];
+acadoWorkspace.state[29] = acadoVariables.od[index * 17 + 4];
+acadoWorkspace.state[30] = acadoVariables.od[index * 17 + 5];
+acadoWorkspace.state[31] = acadoVariables.od[index * 17 + 6];
+acadoWorkspace.state[32] = acadoVariables.od[index * 17 + 7];
+acadoWorkspace.state[33] = acadoVariables.od[index * 17 + 8];
+acadoWorkspace.state[34] = acadoVariables.od[index * 17 + 9];
+acadoWorkspace.state[35] = acadoVariables.od[index * 17 + 10];
+acadoWorkspace.state[36] = acadoVariables.od[index * 17 + 11];
+acadoWorkspace.state[37] = acadoVariables.od[index * 17 + 12];
+acadoWorkspace.state[38] = acadoVariables.od[index * 17 + 13];
+acadoWorkspace.state[39] = acadoVariables.od[index * 17 + 14];
+acadoWorkspace.state[40] = acadoVariables.od[index * 17 + 15];
+acadoWorkspace.state[41] = acadoVariables.od[index * 17 + 16];
acado_integrate(acadoWorkspace.state, index == 0, index);
@@ -5026,24 +5002,23 @@ else
{
acadoWorkspace.state[24] = acadoVariables.u[19];
}
-acadoWorkspace.state[25] = acadoVariables.od[360];
-acadoWorkspace.state[26] = acadoVariables.od[361];
-acadoWorkspace.state[27] = acadoVariables.od[362];
-acadoWorkspace.state[28] = acadoVariables.od[363];
-acadoWorkspace.state[29] = acadoVariables.od[364];
-acadoWorkspace.state[30] = acadoVariables.od[365];
-acadoWorkspace.state[31] = acadoVariables.od[366];
-acadoWorkspace.state[32] = acadoVariables.od[367];
-acadoWorkspace.state[33] = acadoVariables.od[368];
-acadoWorkspace.state[34] = acadoVariables.od[369];
-acadoWorkspace.state[35] = acadoVariables.od[370];
-acadoWorkspace.state[36] = acadoVariables.od[371];
-acadoWorkspace.state[37] = acadoVariables.od[372];
-acadoWorkspace.state[38] = acadoVariables.od[373];
-acadoWorkspace.state[39] = acadoVariables.od[374];
-acadoWorkspace.state[40] = acadoVariables.od[375];
-acadoWorkspace.state[41] = acadoVariables.od[376];
-acadoWorkspace.state[42] = acadoVariables.od[377];
+acadoWorkspace.state[25] = acadoVariables.od[340];
+acadoWorkspace.state[26] = acadoVariables.od[341];
+acadoWorkspace.state[27] = acadoVariables.od[342];
+acadoWorkspace.state[28] = acadoVariables.od[343];
+acadoWorkspace.state[29] = acadoVariables.od[344];
+acadoWorkspace.state[30] = acadoVariables.od[345];
+acadoWorkspace.state[31] = acadoVariables.od[346];
+acadoWorkspace.state[32] = acadoVariables.od[347];
+acadoWorkspace.state[33] = acadoVariables.od[348];
+acadoWorkspace.state[34] = acadoVariables.od[349];
+acadoWorkspace.state[35] = acadoVariables.od[350];
+acadoWorkspace.state[36] = acadoVariables.od[351];
+acadoWorkspace.state[37] = acadoVariables.od[352];
+acadoWorkspace.state[38] = acadoVariables.od[353];
+acadoWorkspace.state[39] = acadoVariables.od[354];
+acadoWorkspace.state[40] = acadoVariables.od[355];
+acadoWorkspace.state[41] = acadoVariables.od[356];
acado_integrate(acadoWorkspace.state, 1, 19);
@@ -5114,24 +5089,23 @@ acadoWorkspace.objValueIn[1] = acadoVariables.x[lRun1 * 4 + 1];
acadoWorkspace.objValueIn[2] = acadoVariables.x[lRun1 * 4 + 2];
acadoWorkspace.objValueIn[3] = acadoVariables.x[lRun1 * 4 + 3];
acadoWorkspace.objValueIn[4] = acadoVariables.u[lRun1];
-acadoWorkspace.objValueIn[5] = acadoVariables.od[lRun1 * 18];
-acadoWorkspace.objValueIn[6] = acadoVariables.od[lRun1 * 18 + 1];
-acadoWorkspace.objValueIn[7] = acadoVariables.od[lRun1 * 18 + 2];
-acadoWorkspace.objValueIn[8] = acadoVariables.od[lRun1 * 18 + 3];
-acadoWorkspace.objValueIn[9] = acadoVariables.od[lRun1 * 18 + 4];
-acadoWorkspace.objValueIn[10] = acadoVariables.od[lRun1 * 18 + 5];
-acadoWorkspace.objValueIn[11] = acadoVariables.od[lRun1 * 18 + 6];
-acadoWorkspace.objValueIn[12] = acadoVariables.od[lRun1 * 18 + 7];
-acadoWorkspace.objValueIn[13] = acadoVariables.od[lRun1 * 18 + 8];
-acadoWorkspace.objValueIn[14] = acadoVariables.od[lRun1 * 18 + 9];
-acadoWorkspace.objValueIn[15] = acadoVariables.od[lRun1 * 18 + 10];
-acadoWorkspace.objValueIn[16] = acadoVariables.od[lRun1 * 18 + 11];
-acadoWorkspace.objValueIn[17] = acadoVariables.od[lRun1 * 18 + 12];
-acadoWorkspace.objValueIn[18] = acadoVariables.od[lRun1 * 18 + 13];
-acadoWorkspace.objValueIn[19] = acadoVariables.od[lRun1 * 18 + 14];
-acadoWorkspace.objValueIn[20] = acadoVariables.od[lRun1 * 18 + 15];
-acadoWorkspace.objValueIn[21] = acadoVariables.od[lRun1 * 18 + 16];
-acadoWorkspace.objValueIn[22] = acadoVariables.od[lRun1 * 18 + 17];
+acadoWorkspace.objValueIn[5] = acadoVariables.od[lRun1 * 17];
+acadoWorkspace.objValueIn[6] = acadoVariables.od[lRun1 * 17 + 1];
+acadoWorkspace.objValueIn[7] = acadoVariables.od[lRun1 * 17 + 2];
+acadoWorkspace.objValueIn[8] = acadoVariables.od[lRun1 * 17 + 3];
+acadoWorkspace.objValueIn[9] = acadoVariables.od[lRun1 * 17 + 4];
+acadoWorkspace.objValueIn[10] = acadoVariables.od[lRun1 * 17 + 5];
+acadoWorkspace.objValueIn[11] = acadoVariables.od[lRun1 * 17 + 6];
+acadoWorkspace.objValueIn[12] = acadoVariables.od[lRun1 * 17 + 7];
+acadoWorkspace.objValueIn[13] = acadoVariables.od[lRun1 * 17 + 8];
+acadoWorkspace.objValueIn[14] = acadoVariables.od[lRun1 * 17 + 9];
+acadoWorkspace.objValueIn[15] = acadoVariables.od[lRun1 * 17 + 10];
+acadoWorkspace.objValueIn[16] = acadoVariables.od[lRun1 * 17 + 11];
+acadoWorkspace.objValueIn[17] = acadoVariables.od[lRun1 * 17 + 12];
+acadoWorkspace.objValueIn[18] = acadoVariables.od[lRun1 * 17 + 13];
+acadoWorkspace.objValueIn[19] = acadoVariables.od[lRun1 * 17 + 14];
+acadoWorkspace.objValueIn[20] = acadoVariables.od[lRun1 * 17 + 15];
+acadoWorkspace.objValueIn[21] = acadoVariables.od[lRun1 * 17 + 16];
acado_evaluateLSQ( acadoWorkspace.objValueIn, acadoWorkspace.objValueOut );
acadoWorkspace.Dy[lRun1 * 5] = acadoWorkspace.objValueOut[0] - acadoVariables.y[lRun1 * 5];
@@ -5144,24 +5118,23 @@ acadoWorkspace.objValueIn[0] = acadoVariables.x[80];
acadoWorkspace.objValueIn[1] = acadoVariables.x[81];
acadoWorkspace.objValueIn[2] = acadoVariables.x[82];
acadoWorkspace.objValueIn[3] = acadoVariables.x[83];
-acadoWorkspace.objValueIn[4] = acadoVariables.od[360];
-acadoWorkspace.objValueIn[5] = acadoVariables.od[361];
-acadoWorkspace.objValueIn[6] = acadoVariables.od[362];
-acadoWorkspace.objValueIn[7] = acadoVariables.od[363];
-acadoWorkspace.objValueIn[8] = acadoVariables.od[364];
-acadoWorkspace.objValueIn[9] = acadoVariables.od[365];
-acadoWorkspace.objValueIn[10] = acadoVariables.od[366];
-acadoWorkspace.objValueIn[11] = acadoVariables.od[367];
-acadoWorkspace.objValueIn[12] = acadoVariables.od[368];
-acadoWorkspace.objValueIn[13] = acadoVariables.od[369];
-acadoWorkspace.objValueIn[14] = acadoVariables.od[370];
-acadoWorkspace.objValueIn[15] = acadoVariables.od[371];
-acadoWorkspace.objValueIn[16] = acadoVariables.od[372];
-acadoWorkspace.objValueIn[17] = acadoVariables.od[373];
-acadoWorkspace.objValueIn[18] = acadoVariables.od[374];
-acadoWorkspace.objValueIn[19] = acadoVariables.od[375];
-acadoWorkspace.objValueIn[20] = acadoVariables.od[376];
-acadoWorkspace.objValueIn[21] = acadoVariables.od[377];
+acadoWorkspace.objValueIn[4] = acadoVariables.od[340];
+acadoWorkspace.objValueIn[5] = acadoVariables.od[341];
+acadoWorkspace.objValueIn[6] = acadoVariables.od[342];
+acadoWorkspace.objValueIn[7] = acadoVariables.od[343];
+acadoWorkspace.objValueIn[8] = acadoVariables.od[344];
+acadoWorkspace.objValueIn[9] = acadoVariables.od[345];
+acadoWorkspace.objValueIn[10] = acadoVariables.od[346];
+acadoWorkspace.objValueIn[11] = acadoVariables.od[347];
+acadoWorkspace.objValueIn[12] = acadoVariables.od[348];
+acadoWorkspace.objValueIn[13] = acadoVariables.od[349];
+acadoWorkspace.objValueIn[14] = acadoVariables.od[350];
+acadoWorkspace.objValueIn[15] = acadoVariables.od[351];
+acadoWorkspace.objValueIn[16] = acadoVariables.od[352];
+acadoWorkspace.objValueIn[17] = acadoVariables.od[353];
+acadoWorkspace.objValueIn[18] = acadoVariables.od[354];
+acadoWorkspace.objValueIn[19] = acadoVariables.od[355];
+acadoWorkspace.objValueIn[20] = acadoVariables.od[356];
acado_evaluateLSQEndTerm( acadoWorkspace.objValueIn, acadoWorkspace.objValueOut );
acadoWorkspace.DyN[0] = acadoWorkspace.objValueOut[0] - acadoVariables.yN[0];
acadoWorkspace.DyN[1] = acadoWorkspace.objValueOut[1] - acadoVariables.yN[1];
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 acc86c78cd..65c198aabd 100755
Binary files a/selfdrive/ui/spinner/spinner and b/selfdrive/ui/spinner/spinner differ
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);
}