retropilot
/
openpilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

openpilot v0.6.3 release

v0.6.3
Vehicle Researcher 2019-08-13 01:36:45 +00:00
parent 6a61788682
commit d5f9caa82d
95 changed files with 1900 additions and 796 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
Dockerfile.openpilot
View File

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

6
README.md
View File

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

11
RELEASES.md
View File

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

BIN
apk/ai.comma.plus.offroad.apk
View File

Binary file not shown.

19
common/api/__init__.py
View File

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

14
installer/updater/Makefile
View File

@ -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 $@)'

8
installer/updater/update.json
View File

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

BIN
installer/updater/updater
View File

Binary file not shown.

113
installer/updater/updater.cc
View File

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

BIN
models/driving_model.dlc
View File

Binary file not shown.

BIN
models/monitoring_model.dlc
View File

Binary file not shown.

4
run_docker_tests.sh
View File

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

BIN
selfdrive/assets/Roboto-Bold.ttf
View File

Binary file not shown.

0
selfdrive/assets/courbd.ttf → selfdrive/assets/fonts/courbd.ttf
View File

0
selfdrive/assets/OpenSans-Bold.ttf → selfdrive/assets/fonts/opensans_bold.ttf
View File

0
selfdrive/assets/OpenSans-Regular.ttf → selfdrive/assets/fonts/opensans_regular.ttf
View File

0
selfdrive/assets/OpenSans-SemiBold.ttf → selfdrive/assets/fonts/opensans_semibold.ttf
View File

BIN
selfdrive/assets/sounds/disengaged.wav
View File

Binary file not shown.

BIN
selfdrive/assets/sounds/engaged.wav
View File

Binary file not shown.

BIN
selfdrive/assets/sounds/error.wav
View File

Binary file not shown.

BIN
selfdrive/assets/sounds/warning_1.wav
View File

Binary file not shown.

BIN
selfdrive/assets/sounds/warning_2.wav
View File

Binary file not shown.

9
selfdrive/athena/athenad.py
View File

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

8
selfdrive/can/tests/test_packer_honda.py
View File

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

12
selfdrive/can/tests/test_packer_toyota.py
View File

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

19
selfdrive/car/chrysler/carcontroller.py
View File

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

10
selfdrive/car/chrysler/chryslercan.py
View File

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

1
selfdrive/car/chrysler/chryslercan_test.py
View File

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

3
selfdrive/car/chrysler/interface.py
View File

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

2
selfdrive/car/chrysler/values.py
View File

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

20
selfdrive/car/gm/carcontroller.py
View File

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

4
selfdrive/car/gm/gmcan.py
View File

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

7
selfdrive/car/gm/interface.py
View File

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

21
selfdrive/car/gm/values.py
View File

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

14
selfdrive/car/honda/carcontroller.py
View File

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

5
selfdrive/car/honda/carstate.py
View File

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

11
selfdrive/car/honda/hondacan.py
View File

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

14
selfdrive/car/honda/interface.py
View File

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

26
selfdrive/car/honda/values.py
View File

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

2
selfdrive/car/hyundai/interface.py
View File

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

5
selfdrive/car/hyundai/values.py
View File

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

23
selfdrive/car/toyota/carcontroller.py
View File

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

33
selfdrive/car/toyota/interface.py
View File

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

6
selfdrive/car/toyota/toyotacan.py
View File

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

2
selfdrive/common/version.h
View File

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

33
selfdrive/controls/controlsd.py
View File

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

9
selfdrive/controls/lib/alertmanager.py
View File

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

38
selfdrive/controls/lib/alerts.py
View File

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

109
selfdrive/controls/lib/driver_monitor.py
View File

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

74
selfdrive/controls/lib/lane_planner.py 100644
View File

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

27
selfdrive/controls/lib/latcontrol_helpers.py
View File

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

2
selfdrive/controls/lib/latcontrol_indi.py
View File

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

72
selfdrive/controls/lib/latcontrol_lqr.py 100644
View File

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

2
selfdrive/controls/lib/latcontrol_pid.py
View File

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

35
selfdrive/controls/lib/lateral_mpc/generator.cpp
View File

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

15
selfdrive/controls/lib/lateral_mpc/lateral_mpc.c
View File

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

22
selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_common.h
View File

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

1
selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_integrator.c
View File

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

345
selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_solver.c
View File

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

4
selfdrive/controls/lib/lateral_mpc/libmpc_py.py
View File

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

66
selfdrive/controls/lib/model_parser.py
View File

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

47
selfdrive/controls/lib/pathplanner.py
View File

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

44
selfdrive/controls/lib/planner.py
View File

@ -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'].path.poly):
path = list(sm['model'].path.poly)
if len(sm['model'].speed):
self.model_v_kf.update(sm['model'].speed[SPEED_PERCENTILE_IDX])
# 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
if self.params.get("LimitSetSpeedNeural") == "1":
model_speed = self.model_v_kf.x[0][0]
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

30
selfdrive/controls/lib/radar_helpers.py
View File

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

11
selfdrive/controls/radard.py
View File

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

20
selfdrive/controls/tests/test_lateral_mpc.py
View File

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

10
selfdrive/locationd/calibrationd.py
View File

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

19
selfdrive/locationd/locationd_yawrate.cc
View File

@ -12,9 +12,11 @@
void Localizer::update_state(const Eigen::Matrix<double, 1, 2> &C, const double R, double current_time, double meas) {
double dt = current_time - prev_update_time;
prev_update_time = current_time;
if (dt < 1.0e-9) {
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;

9
selfdrive/locationd/paramsd.cc
View File

@ -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();

21
selfdrive/loggerd/uploader.py
View File

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

5
selfdrive/manager.py
View File

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

4
selfdrive/registration.py
View File

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

22
selfdrive/test/openpilotci_upload.py 100755
View File

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

29
selfdrive/test/plant/plant.py
View File

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

494
selfdrive/test/test_car_models_openpilot.py 100755
View File

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

0
selfdrive/test/tests/__init__.py 100644
View File

2
selfdrive/test/tests/process_replay/.gitignore vendored 100644
View File

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

15
selfdrive/test/tests/process_replay/README.md 100644
View File

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

0
selfdrive/test/tests/process_replay/__init__.py 100644
View File

45
selfdrive/test/tests/process_replay/compare_logs.py 100755
View File

@ -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:])

185
selfdrive/test/tests/process_replay/process_replay.py 100755
View File

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

1
selfdrive/test/tests/process_replay/ref_commit 100644
View File

@ -0,0 +1 @@
e3388c62ffb80f4b3ca8721da56a581a93c44e79

118
selfdrive/test/tests/process_replay/test_processes.py 100755
View File

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

42
selfdrive/test/tests/process_replay/update_refs.py 100755
View File

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

2
selfdrive/ui/slplay.c
View File

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

BIN
selfdrive/ui/spinner/spinner
View File

Binary file not shown.

2
selfdrive/ui/spinner/spinner.c
View File

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

88
selfdrive/ui/ui.c
View File

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

5
selfdrive/visiond/build_from_src.mk
View File

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

65
selfdrive/visiond/models/driving.cc
View File

@ -4,36 +4,32 @@
#include <unistd.h>
#ifdef QCOM
#include <eigen3/Eigen/Dense>
#include <eigen3/Eigen/Dense>
#else
#include <Eigen/Dense>
#include <Eigen/Dense>
#endif
#include "common/timing.h"
#include "driving.h"
#ifdef MEDMODEL
#define MODEL_WIDTH 512
#define MODEL_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<float, MODEL_PATH_DISTANCE, POLYFIT_DEGREE> vander;
void model_init(ModelState* s, cl_device_id device_id, cl_context context, int temporal) {
@ -70,6 +66,13 @@ ModelData model_eval_frame(ModelState* s, cl_command_queue q,
float *net_input_buf = model_input_prepare(&s->in, q, yuv_cl, width, height, transform);
#ifdef DUMP_YUV
FILE *dump_yuv_file = fopen("/sdcard/dump.yuv", "wb");
fwrite(net_input_buf, MODEL_HEIGHT*MODEL_WIDTH*3/2, sizeof(float), dump_yuv_file);
fclose(dump_yuv_file);
assert(1==2);
#endif
//printf("readinggggg \n");
//FILE *f = fopen("goof_frame", "r");
//fread(net_input_buf, sizeof(float), MODEL_HEIGHT*MODEL_WIDTH*3/2, f);
@ -84,7 +87,7 @@ ModelData model_eval_frame(ModelState* s, cl_command_queue q,
net_outputs.left_lane = &s->output[MODEL_PATH_DISTANCE*2];
net_outputs.right_lane = &s->output[MODEL_PATH_DISTANCE*2 + MODEL_PATH_DISTANCE*2 + 1];
net_outputs.lead = &s->output[MODEL_PATH_DISTANCE*2 + (MODEL_PATH_DISTANCE*2 + 1)*2];
net_outputs.speed = &s->output[OUTPUT_SIZE - SPEED_BUCKETS];
//net_outputs.speed = &s->output[OUTPUT_SIZE - SPEED_BUCKETS];
ModelData model = {0};
@ -111,7 +114,11 @@ ModelData model_eval_frame(ModelState* s, cl_command_queue q,
const double max_dist = 140.0;
const double max_rel_vel = 10.0;
// current lead
// Every output distribution from the MDN includes the probabilties
// of it representing a current lead car, a lead car in 2s
// or a lead car in 4s
// Find the distribution that corresponds to the current lead
int mdn_max_idx = 0;
for (int i=1; i<LEAD_MDN_N; i++) {
if (net_outputs.lead[i*MDN_GROUP_SIZE + 8] > net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + 8]) {
@ -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<LEAD_MDN_N; i++) {
if (net_outputs.lead[i*MDN_GROUP_SIZE + 9] > net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + 9]) {
@ -150,20 +157,20 @@ ModelData model_eval_frame(ModelState* s, cl_command_queue q,
for (int i=0; i < SPEED_PERCENTILES; i++) {
model.speed[i] = ((float) SPEED_BUCKETS)/2.0;
}
float sum = 0;
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;
}

1
selfdrive/visiond/models/driving.h
View File

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

2
selfdrive/visiond/models/monitoring.cc
View File

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

4
selfdrive/visiond/models/monitoring.h
View File

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

5
selfdrive/visiond/runners/run.h
View File

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

59
selfdrive/visiond/visiond.cc
View File

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