diff --git a/.gitattributes b/.gitattributes index 0a17bd17a8d065..7a21b223d0290d 100644 --- a/.gitattributes +++ b/.gitattributes @@ -1,9 +1,2 @@ -*.keras filter=lfs diff=lfs merge=lfs -text *.dlc filter=lfs diff=lfs merge=lfs -text *.onnx filter=lfs diff=lfs merge=lfs -text -*.pb filter=lfs diff=lfs merge=lfs -text -*.bin filter=lfs diff=lfs merge=lfs -text -*.jpg filter=lfs diff=lfs merge=lfs -text -.media/df_profiles.jpg !filter !diff !merge text -*.ipynb filter=nbstripout -diff -external/ffmpeg/bin/ffmpeg_cuda filter=lfs diff=lfs merge=lfs -text diff --git a/.github/workflows/selfdrive_tests.yaml b/.github/workflows/selfdrive_tests.yaml index f647df9c16fa65..31b1b997f7f15a 100644 --- a/.github/workflows/selfdrive_tests.yaml +++ b/.github/workflows/selfdrive_tests.yaml @@ -260,12 +260,14 @@ jobs: $UNIT_TEST tools/lib/tests && \ ./selfdrive/boardd/tests/test_boardd_usbprotocol && \ ./selfdrive/common/tests/test_util && \ + ./selfdrive/common/tests/test_swaglog && \ ./selfdrive/loggerd/tests/test_logger &&\ ./selfdrive/proclogd/tests/test_proclog && \ ./selfdrive/ui/replay/tests/test_replay && \ - ./selfdrive/camerad/test/ae_gray_test" - - name: Upload coverage to Codecov - run: bash <(curl -s https://codecov.io/bash) -v -F unit_tests + ./selfdrive/camerad/test/ae_gray_test && \ + coverage xml" + - name: "Upload coverage to Codecov" + uses: codecov/codecov-action@v2 # process_replay: # name: process replay @@ -298,9 +300,10 @@ jobs: # - name: Run replay # run: | # ${{ env.RUN }} "scons -j$(nproc) && \ -# FILEREADER_CACHE=1 CI=1 coverage run selfdrive/test/process_replay/test_processes.py" -# - name: Upload coverage to Codecov -# run: bash <(curl -s https://codecov.io/bash) -v -F process_replay +# FILEREADER_CACHE=1 CI=1 coverage run selfdrive/test/process_replay/test_processes.py && \ +# coverage xml" +# - name: "Upload coverage to Codecov" +# uses: codecov/codecov-action@v2 # - name: Print diff # if: always() # run: cat selfdrive/test/process_replay/diff.txt @@ -353,7 +356,10 @@ jobs: ${{ env.RUN }} "mkdir -p selfdrive/test/out && \ scons -j$(nproc) && \ cd selfdrive/test/longitudinal_maneuvers && \ - ./test_longitudinal.py" + coverage run ./test_longitudinal.py && \ + coverage xml" + - name: "Upload coverage to Codecov" + uses: codecov/codecov-action@v2 - uses: actions/upload-artifact@v2 if: always() continue-on-error: true @@ -396,13 +402,14 @@ jobs: - name: Test car models run: | ${{ env.RUN }} "scons -j$(nproc) --test && \ - FILEREADER_CACHE=1 pytest selfdrive/test/test_models.py && \ + FILEREADER_CACHE=1 coverage run -m pytest selfdrive/test/test_models.py && \ + coverage xml && \ chmod -R 777 /tmp/comma_download_cache" env: NUM_JOBS: 4 JOB_ID: ${{ matrix.job }} - - name: Upload coverage to Codecov - run: bash <(curl -s https://codecov.io/bash) -v -F test_car_models + - name: "Upload coverage to Codecov" + uses: codecov/codecov-action@v2 docs: name: build docs diff --git a/.lfsconfig b/.lfsconfig index 460dd4b0b45d37..42dfa2d9448676 100644 --- a/.lfsconfig +++ b/.lfsconfig @@ -1,3 +1,4 @@ [lfs] url = https://gitlab.com/commaai/openpilot-lfs.git/info/lfs - pushurl = + pushurl = ssh://git@gitlab.com/commaai/openpilot-lfs.git + locksverify = false diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 65b60d95ce80ec..3fc559656820fb 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -1,6 +1,10 @@ repos: +- repo: meta + hooks: + - id: check-hooks-apply + - id: check-useless-excludes - repo: https://github.com/pre-commit/pre-commit-hooks - rev: v4.0.1 + rev: v4.1.0 hooks: - id: check-ast exclude: '^(pyextra)/' @@ -9,8 +13,10 @@ repos: - id: check-yaml - id: check-merge-conflict - id: check-symlinks + - id: check-added-large-files + args: ['--maxkb=100'] - repo: https://github.com/pre-commit/mirrors-mypy - rev: v0.910-1 + rev: v0.931 hooks: - id: mypy exclude: '^(pyextra/)|(cereal/)|(rednose/)|(panda/)|(laika/)|(opendbc/)|(laika_repo/)|(rednose_repo/)/' @@ -21,9 +27,13 @@ repos: hooks: - id: flake8 exclude: '^(pyextra/)|(cereal/)|(rednose/)|(panda/)|(laika/)|(opendbc/)|(laika_repo/)|(rednose_repo/)|(selfdrive/debug/)/' + additional_dependencies: ['flake8-no-implicit-concat'] args: + - --indent-size=2 + - --enable-extensions=NIC - --select=F,E112,E113,E304,E502,E701,E702,E703,E71,E72,E731,W191,W6 - --statistics + - -j4 - repo: local hooks: - id: pylint @@ -31,7 +41,7 @@ repos: entry: pylint language: system types: [python] - exclude: '^(pyextra/)|(cereal/)|(rednose/)|(panda/)|(laika/)|(laika_repo/)|(rednose_repo/)/' + exclude: '^(pyextra/)|(cereal/)|(rednose/)|(panda/)|(laika/)|(laika_repo/)|(rednose_repo/)' - repo: local hooks: - id: cppcheck diff --git a/.pylintrc b/.pylintrc index a3a2fc36f94c6c..d7edeb11733815 100644 --- a/.pylintrc +++ b/.pylintrc @@ -463,6 +463,11 @@ known-standard-library= # Force import order to recognize a module as part of a third party library. known-third-party=enchant +[STRING] + +# This flag controls whether the implicit-str-concat should generate a warning +# on implicit string concatenation in sequences defined over several lines. +check-str-concat-over-line-jumps=yes [EXCEPTIONS] diff --git a/Dockerfile.openpilot_base b/Dockerfile.openpilot_base index 9229fb176598fa..2ea26b854f4a90 100644 --- a/Dockerfile.openpilot_base +++ b/Dockerfile.openpilot_base @@ -13,14 +13,20 @@ ENV LANGUAGE en_US:en ENV LC_ALL en_US.UTF-8 ENV PIPENV_SYSTEM=1 +ENV PYENV_VERSION=3.8.10 +ENV PYENV_ROOT="/root/.pyenv" +ENV PATH="$PYENV_ROOT/bin:$PYENV_ROOT/shims:$PATH" + COPY Pipfile Pipfile.lock .python-version update_requirements.sh /tmp/ COPY tools/ubuntu_setup.sh /tmp/tools/ RUN cd /tmp && \ tools/ubuntu_setup.sh && \ - rm -rf /tmp/* && \ rm -rf /var/lib/apt/lists/* && \ - pip uninstall -y pipenv + rm -rf /tmp/* && \ + rm -rf /root/.cache && \ + pip uninstall -y pipenv && \ -ENV PYENV_VERSION=3.8.10 -ENV PYENV_ROOT="/root/.pyenv" -ENV PATH="$PYENV_ROOT/bin:$PYENV_ROOT/shims:$PATH" + # remove unused architectures from gcc for panda + cd /usr/lib/gcc/arm-none-eabi/9.2.1 && \ + rm -rf arm/ && \ + rm -rf thumb/nofp thumb/v6* thumb/v8* thumb/v7+fp thumb/v7-r+fp.sp diff --git a/RELEASES.md b/RELEASES.md index efbd5bf91cd67f..e65aa21fbc83e8 100644 --- a/RELEASES.md +++ b/RELEASES.md @@ -1,10 +1,13 @@ Version 0.8.13 (2022-XX-XX) ======================== * Improved driver monitoring + * Roll compensation * Improved camera focus on the comma two * Subaru ECU firmware fingerprinting thanks to martinl! * Hyundai Santa Fe Plug-in Hybrid 2022 support thanks to sunnyhaibin! * Subaru Impreza 2020 support thanks to martinl! + * Toyota Avalon 2022 support thanks to sshane! + * Toyota Prius v 2017 support thanks to CT921! Version 0.8.12 (2021-12-15) ======================== diff --git a/cereal b/cereal index 485a454e8a3689..315c5ddbcb7d96 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit 485a454e8a36897721b21e81196f4ee4527a9042 +Subproject commit 315c5ddbcb7d96c96c183a4f055720e724bd213e diff --git a/common/file_helpers.py b/common/file_helpers.py index 3373d82a03daaa..592b2a199a75e5 100644 --- a/common/file_helpers.py +++ b/common/file_helpers.py @@ -81,6 +81,17 @@ def _get_fileobject(): return writer.get_fileobject(dir=temp_dir) return _get_fileobject +def monkeypatch_os_link(): + # This is neccesary on EON/C2, where os.link is patched out of python + if not hasattr(os, 'link'): + from cffi import FFI + ffi = FFI() + ffi.cdef("int link(const char *oldpath, const char *newpath);") + libc = ffi.dlopen(None) + + def link(src, dest): + return libc.link(src.encode(), dest.encode()) + os.link = link def atomic_write_on_fs_tmp(path, **kwargs): """Creates an atomic writer using a temporary file in a temporary directory @@ -88,6 +99,7 @@ def atomic_write_on_fs_tmp(path, **kwargs): """ # TODO(mgraczyk): This use of AtomicWriter relies on implementation details to set the temp # directory. + monkeypatch_os_link() writer = AtomicWriter(path, **kwargs) return writer._open(_get_fileobject_func(writer, get_tmpdir_on_same_filesystem(path))) @@ -96,5 +108,6 @@ def atomic_write_in_dir(path, **kwargs): """Creates an atomic writer using a temporary file in the same directory as the destination file. """ + monkeypatch_os_link() writer = AtomicWriter(path, **kwargs) return writer._open(_get_fileobject_func(writer, os.path.dirname(path))) diff --git a/common/params_pyx.pyx b/common/params_pyx.pyx index 7b35e815dfd973..a8c43c2f2f86eb 100755 --- a/common/params_pyx.pyx +++ b/common/params_pyx.pyx @@ -8,7 +8,6 @@ cdef extern from "selfdrive/common/params.h": cpdef enum ParamKeyType: PERSISTENT CLEAR_ON_MANAGER_START - CLEAR_ON_PANDA_DISCONNECT CLEAR_ON_IGNITION_ON CLEAR_ON_IGNITION_OFF ALL diff --git a/common/tests/test_params.py b/common/tests/test_params.py index 0b3d8dbda06260..906198871bfa5e 100644 --- a/common/tests/test_params.py +++ b/common/tests/test_params.py @@ -24,14 +24,6 @@ def test_params_non_ascii(self): self.params.put("CarParams", st) assert self.params.get("CarParams") == st - def test_params_get_cleared_panda_disconnect(self): - self.params.put("CarParams", "test") - self.params.put("DongleId", "cb38263377b873ee") - assert self.params.get("CarParams") == b"test" - self.params.clear_all(ParamKeyType.CLEAR_ON_PANDA_DISCONNECT) - assert self.params.get("CarParams") is None - assert self.params.get("DongleId") is not None - def test_params_get_cleared_manager_start(self): self.params.put("CarParams", "test") self.params.put("DongleId", "cb38263377b873ee") diff --git a/docs/CARS.md b/docs/CARS.md index ebaf6556f5ec37..b2a8c387a0a8e0 100644 --- a/docs/CARS.md +++ b/docs/CARS.md @@ -43,7 +43,7 @@ | Lexus | RX Hybrid 2016-19 | All | Stock3| 0mph | 0mph | | Lexus | RX Hybrid 2020-21 | All | openpilot | 0mph | 0mph | | Lexus | UX Hybrid 2019-21 | All | openpilot | 0mph | 0mph | -| Toyota | Alphard 2020 | All | openpilot | 0mph | 0mph | +| Toyota | Alphard 2019-20 | All | openpilot | 0mph | 0mph | | Toyota | Avalon 2016-21 | TSS-P | Stock3| 20mph1 | 0mph | | Toyota | Avalon 2022 | All | openpilot | 0mph | 0mph | | Toyota | Avalon Hybrid 2019-21 | TSS-P | Stock3| 20mph1 | 0mph | @@ -64,6 +64,7 @@ | Toyota | Mirai 2021 | All | openpilot | 0mph | 0mph | | Toyota | Prius 2016-20 | TSS-P | Stock3| 0mph | 0mph | | Toyota | Prius 2021-22 | All | openpilot | 0mph | 0mph | +| Toyota | Prius v 2017 | TSS-P | Stock3| 20mph1 | 0mph | | Toyota | Prius Prime 2017-20 | All | Stock3| 0mph | 0mph | | Toyota | Prius Prime 2021-22 | All | openpilot | 0mph | 0mph | | Toyota | Rav4 2016-18 | TSS-P | Stock3| 20mph1 | 0mph | @@ -79,99 +80,95 @@ ## Community Maintained Cars and Features -| Make | Model (US Market Reference) | Supported Package | ACC | No ACC accel below | No ALC below | -| ----------| ------------------------------| ------------------| -----------------| -------------------| -------------| -| Audi | A3 2014-19 | ACC + Lane Assist | Stock | 0mph | 0mph | -| Audi | A3 Sportback e-tron 2017-18 | ACC + Lane Assist | Stock | 0mph | 0mph | -| Audi | Q2 2018 | ACC + Lane Assist | Stock | 0mph | 0mph | -| Audi | Q3 2020-21 | ACC + Lane Assist | Stock | 0mph | 0mph | -| Audi | S3 2015 | ACC + Lane Assist | Stock | 0mph | 0mph | -| Buick | Regal 20181 | Adaptive Cruise | openpilot | 0mph | 7mph | -| Cadillac | ATS 20181 | Adaptive Cruise | openpilot | 0mph | 7mph | -| Cadillac | Escalade ESV 20161 | ACC + LKAS | openpilot | 0mph | 7mph | -| Chevrolet | Malibu 20171 | Adaptive Cruise | openpilot | 0mph | 7mph | -| Chevrolet | Volt 2017-181 | Adaptive Cruise | openpilot | 0mph | 7mph | -| Chrysler | Pacifica 2017-18 | Adaptive Cruise | Stock | 0mph | 9mph | -| Chrysler | Pacifica 2020 | Adaptive Cruise | Stock | 0mph | 39mph | -| Chrysler | Pacifica Hybrid 2017-18 | Adaptive Cruise | Stock | 0mph | 9mph | -| Chrysler | Pacifica Hybrid 2019-21 | Adaptive Cruise | Stock | 0mph | 39mph | -| Genesis | G70 2018 | All | Stock | 0mph | 0mph | -| Genesis | G70 2020 | All | Stock | 0mph | 0mph | -| Genesis | G80 2018 | All | Stock | 0mph | 0mph | -| Genesis | G90 2018 | All | Stock | 0mph | 0mph | -| GMC | Acadia 20181 | Adaptive Cruise | openpilot | 0mph | 7mph | -| Holden | Astra 20171 | Adaptive Cruise | openpilot | 0mph | 7mph | -| Hyundai | Elantra 2017-19 | SCC + LKAS | Stock | 19mph | 34mph | -| Hyundai | Elantra 2021 | SCC + LKAS | Stock | 0mph | 0mph | -| Hyundai | Elantra Hybrid 2021 | SCC + LKAS | Stock | 0mph | 0mph | -| Hyundai | Genesis 2015-16 | SCC + LKAS | Stock | 19mph | 37mph | -| Hyundai | Ioniq Electric 2019 | SCC + LKAS | Stock | 0mph | 32mph | -| Hyundai | Ioniq Electric 2020 | SCC + LKAS | Stock | 0mph | 0mph | -| Hyundai | Ioniq Hybrid 2017-19 | SCC + LKAS | Stock | 0mph | 32mph | -| Hyundai | Ioniq Hybrid 2020-22 | SCC + LFA | Stock | 0mph | 0mph | -| Hyundai | Ioniq PHEV 2020-21 | SCC + LKAS | Stock | 0mph | 0mph | -| Hyundai | Kona 2020 | SCC + LKAS | Stock | 0mph | 0mph | -| Hyundai | Kona EV 2018-19 | SCC + LKAS | Stock | 0mph | 0mph | -| Hyundai | Kona Hybrid 2020 | SCC + LKAS | Stock | 0mph | 0mph | -| Hyundai | Santa Fe 2019-20 | All | Stock | 0mph | 0mph | -| Hyundai | Santa Fe 2021-22 | All | Stock | 0mph | 0mph | -| Hyundai | Santa Fe Hybrid 2022 | All | Stock | 0mph | 0mph | -| Hyundai | Santa Fe Plug-in Hybrid 2022 | All | Stock | 0mph | 0mph | -| Hyundai | Sonata 2018-2019 | SCC + LKAS | Stock | 0mph | 0mph | -| Hyundai | Sonata Hybrid 2021-22 | All | Stock | 0mph | 0mph | -| Hyundai | Veloster 2019-20 | SCC + LKAS | Stock | 5mph | 0mph | -| Jeep | Grand Cherokee 2016-18 | Adaptive Cruise | Stock | 0mph | 9mph | -| Jeep | Grand Cherokee 2019-20 | Adaptive Cruise | Stock | 0mph | 39mph | -| Kia | Ceed 2019 | SCC + LKAS | Stock | 0mph | 0mph | -| Kia | Forte 2018-21 | SCC + LKAS | Stock | 0mph | 0mph | -| Kia | K5 2021-22 | SCC + LFA | Stock | 0mph | 0mph | -| Kia | Niro EV 2019-22 | All | Stock | 0mph | 0mph | -| Kia | Niro Hybrid 2021 | SCC + LKAS | Stock | 0mph | 0mph | -| Kia | Niro PHEV 2019 | SCC + LKAS | Stock | 10mph | 32mph | -| Kia | Optima 2017 | SCC + LKAS | Stock | 0mph | 32mph | -| Kia | Optima 2019 | SCC + LKAS | Stock | 0mph | 0mph | -| Kia | Seltos 2021 | SCC + LKAS | Stock | 0mph | 0mph | -| Kia | Sorento 2018-19 | SCC + LKAS | Stock | 0mph | 0mph | -| Kia | Stinger 2018 | SCC + LKAS | Stock | 0mph | 0mph | -| Kia | Telluride 2020 | SCC + LKAS | Stock | 0mph | 0mph | -| Mazda | CX-9 2021 | All | Stock | 0mph | 28mph | -| Nissan | Altima 2019-20 | ProPILOT | Stock | 0mph | 0mph | -| Nissan | Leaf 2018-22 | ProPILOT | Stock | 0mph | 0mph | -| Nissan | Rogue 2018-20 | ProPILOT | Stock | 0mph | 0mph | -| Nissan | X-Trail 2017 | ProPILOT | Stock | 0mph | 0mph | -| SEAT | Ateca 2018 | Driver Assistance | Stock | 0mph | 0mph | -| SEAT | Leon 2014-2020 | Driver Assistance | Stock | 0mph | 0mph | -| Škoda | Kamiq 20212 | Driver Assistance | Stock | 0mph | 0mph | -| Škoda | Karoq 2019 | Driver Assistance | Stock | 0mph | 0mph | -| Škoda | Kodiaq 2018-19 | Driver Assistance | Stock | 0mph | 0mph | -| Škoda | Octavia 2015, 2018-19 | Driver Assistance | Stock | 0mph | 0mph | -| Škoda | Octavia RS 2016 | Driver Assistance | Stock | 0mph | 0mph | -| Škoda | Scala 2020 | Driver Assistance | Stock | 0mph | 0mph | -| Škoda | Superb 2015-18 | Driver Assistance | Stock | 0mph | 0mph | -| Subaru | Ascent 2019 | EyeSight | Stock | 0mph | 0mph | -| Subaru | Crosstrek 2018-20 | EyeSight | Stock | 0mph | 0mph | -| Subaru | Forester 2019-21 | EyeSight | Stock | 0mph | 0mph | -| Subaru | Impreza 2017-19 | EyeSight | Stock | 0mph | 0mph | -| Volkswagen| Arteon 2018, 20214 | Driver Assistance | Stock | 0mph | 0mph | -| Volkswagen| Atlas 2018-19 | Driver Assistance | Stock | 0mph | 0mph | -| Volkswagen| California 20214 | Driver Assistance | Stock | 0mph | 32mph | -| Volkswagen| e-Golf 2014, 2019-20 | Driver Assistance | Stock | 0mph | 0mph | -| Volkswagen| Golf 2015-20 | Driver Assistance | Stock | 0mph | 0mph | -| Volkswagen| Golf Alltrack 2017-18 | Driver Assistance | Stock | 0mph | 0mph | -| Volkswagen| Golf GTE 2016 | Driver Assistance | Stock | 0mph | 0mph | -| Volkswagen| Golf GTI 2018-20 | Driver Assistance | Stock | 0mph | 0mph | -| Volkswagen| Golf R 2016-19 | Driver Assistance | Stock | 0mph | 0mph | -| Volkswagen| Golf SportsVan 2016 | Driver Assistance | Stock | 0mph | 0mph | -| Volkswagen| Golf SportWagen 2015 | Driver Assistance | Stock | 0mph | 0mph | -| Volkswagen| Jetta 2018-20 | Driver Assistance | Stock | 0mph | 0mph | -| Volkswagen| Jetta GLI 2021 | Driver Assistance | Stock | 0mph | 0mph | -| Volkswagen| Passat 2016-183 | Driver Assistance | Stock | 0mph | 0mph | -| Volkswagen| Polo 2020 | Driver Assistance | Stock | 0mph | 0mph | -| Volkswagen| T-Cross 20214 | Driver Assistance | Stock | 0mph | 0mph | -| Volkswagen| T-Roc 20214 | Driver Assistance | Stock | 0mph | 0mph | -| Volkswagen| Taos 20224 | Driver Assistance | Stock | 0mph | 0mph | -| Volkswagen| Tiguan 2020 | Driver Assistance | Stock | 0mph | 0mph | -| Volkswagen| Touran 2017 | Driver Assistance | Stock | 0mph | 0mph | +| Make | Model (US Market Reference) | Supported Package | ACC | No ACC accel below | No ALC below | +| ----------| --------------------------------| ------------------| -----------------| -------------------| -------------| +| Audi | A3 2014-19 | ACC + Lane Assist | Stock | 0mph | 0mph | +| Audi | A3 Sportback e-tron 2017-18 | ACC + Lane Assist | Stock | 0mph | 0mph | +| Audi | Q2 2018 | ACC + Lane Assist | Stock | 0mph | 0mph | +| Audi | Q3 2020-21 | ACC + Lane Assist | Stock | 0mph | 0mph | +| Audi | S3 2015 | ACC + Lane Assist | Stock | 0mph | 0mph | +| Cadillac | Escalade ESV 20161 | ACC + LKAS | openpilot | 0mph | 7mph | +| Chevrolet | Volt 2017-181 | Adaptive Cruise | openpilot | 0mph | 7mph | +| Chrysler | Pacifica 2017-18 | Adaptive Cruise | Stock | 0mph | 9mph | +| Chrysler | Pacifica 2020 | Adaptive Cruise | Stock | 0mph | 39mph | +| Chrysler | Pacifica Hybrid 2017-18 | Adaptive Cruise | Stock | 0mph | 9mph | +| Chrysler | Pacifica Hybrid 2019-21 | Adaptive Cruise | Stock | 0mph | 39mph | +| Genesis | G70 2018 | All | Stock | 0mph | 0mph | +| Genesis | G70 2020 | All | Stock | 0mph | 0mph | +| Genesis | G80 2018 | All | Stock | 0mph | 0mph | +| Genesis | G90 2018 | All | Stock | 0mph | 0mph | +| GMC | Acadia 20181 | Adaptive Cruise | openpilot | 0mph | 7mph | +| Hyundai | Elantra 2017-19 | SCC + LKAS | Stock | 19mph | 34mph | +| Hyundai | Elantra 2021 | SCC + LKAS | Stock | 0mph | 0mph | +| Hyundai | Elantra Hybrid 2021 | SCC + LKAS | Stock | 0mph | 0mph | +| Hyundai | Genesis 2015-16 | SCC + LKAS | Stock | 19mph | 37mph | +| Hyundai | Ioniq Electric 2019 | SCC + LKAS | Stock | 0mph | 32mph | +| Hyundai | Ioniq Electric 2020 | SCC + LKAS | Stock | 0mph | 0mph | +| Hyundai | Ioniq Hybrid 2017-19 | SCC + LKAS | Stock | 0mph | 32mph | +| Hyundai | Ioniq Hybrid 2020-22 | SCC + LFA | Stock | 0mph | 0mph | +| Hyundai | Ioniq PHEV 2020-21 | SCC + LKAS | Stock | 0mph | 0mph | +| Hyundai | Kona 2020 | SCC + LKAS | Stock | 0mph | 0mph | +| Hyundai | Kona EV 2018-19 | SCC + LKAS | Stock | 0mph | 0mph | +| Hyundai | Kona Hybrid 2020 | SCC + LKAS | Stock | 0mph | 0mph | +| Hyundai | Santa Fe 2019-20 | All | Stock | 0mph | 0mph | +| Hyundai | Santa Fe 2021-22 | All | Stock | 0mph | 0mph | +| Hyundai | Santa Fe Hybrid 2022 | All | Stock | 0mph | 0mph | +| Hyundai | Santa Fe Plug-in Hybrid 2022 | All | Stock | 0mph | 0mph | +| Hyundai | Sonata 2018-2019 | SCC + LKAS | Stock | 0mph | 0mph | +| Hyundai | Sonata Hybrid 2021-22 | All | Stock | 0mph | 0mph | +| Hyundai | Veloster 2019-20 | SCC + LKAS | Stock | 5mph | 0mph | +| Jeep | Grand Cherokee 2016-18 | Adaptive Cruise | Stock | 0mph | 9mph | +| Jeep | Grand Cherokee 2019-20 | Adaptive Cruise | Stock | 0mph | 39mph | +| Kia | Ceed 2019 | SCC + LKAS | Stock | 0mph | 0mph | +| Kia | Forte 2018-21 | SCC + LKAS | Stock | 0mph | 0mph | +| Kia | K5 2021-22 | SCC + LFA | Stock | 0mph | 0mph | +| Kia | Niro EV 2019-22 | All | Stock | 0mph | 0mph | +| Kia | Niro Hybrid 2021 | SCC + LKAS | Stock | 0mph | 0mph | +| Kia | Niro PHEV 2019 | SCC + LKAS | Stock | 10mph | 32mph | +| Kia | Optima 2017 | SCC + LKAS | Stock | 0mph | 32mph | +| Kia | Optima 2019 | SCC + LKAS | Stock | 0mph | 0mph | +| Kia | Seltos 2021 | SCC + LKAS | Stock | 0mph | 0mph | +| Kia | Sorento 2018-19 | SCC + LKAS | Stock | 0mph | 0mph | +| Kia | Stinger 2018 | SCC + LKAS | Stock | 0mph | 0mph | +| Kia | Telluride 2020 | SCC + LKAS | Stock | 0mph | 0mph | +| Mazda | CX-9 2021 | All | Stock | 0mph | 28mph | +| Nissan | Altima 2019-20 | ProPILOT | Stock | 0mph | 0mph | +| Nissan | Leaf 2018-22 | ProPILOT | Stock | 0mph | 0mph | +| Nissan | Rogue 2018-20 | ProPILOT | Stock | 0mph | 0mph | +| Nissan | X-Trail 2017 | ProPILOT | Stock | 0mph | 0mph | +| SEAT | Ateca 2018 | Driver Assistance | Stock | 0mph | 0mph | +| SEAT | Leon 2014-2020 | Driver Assistance | Stock | 0mph | 0mph | +| Subaru | Ascent 2019 | EyeSight | Stock | 0mph | 0mph | +| Subaru | Crosstrek 2018-20 | EyeSight | Stock | 0mph | 0mph | +| Subaru | Forester 2019-21 | EyeSight | Stock | 0mph | 0mph | +| Subaru | Impreza 2017-19 | EyeSight | Stock | 0mph | 0mph | +| Škoda | Kamiq 20212 | Driver Assistance | Stock | 0mph | 0mph | +| Škoda | Karoq 2019 | Driver Assistance | Stock | 0mph | 0mph | +| Škoda | Kodiaq 2018-19 | Driver Assistance | Stock | 0mph | 0mph | +| Škoda | Octavia 2015, 2018-19 | Driver Assistance | Stock | 0mph | 0mph | +| Škoda | Octavia RS 2016 | Driver Assistance | Stock | 0mph | 0mph | +| Škoda | Scala 2020 | Driver Assistance | Stock | 0mph | 0mph | +| Škoda | Superb 2015-18 | Driver Assistance | Stock | 0mph | 0mph | +| Volkswagen| Arteon 2018, 20214 | Driver Assistance | Stock | 0mph | 0mph | +| Volkswagen| Atlas 2018-19, 20224 | Driver Assistance | Stock | 0mph | 0mph | +| Volkswagen| California 20214 | Driver Assistance | Stock | 0mph | 32mph | +| Volkswagen| e-Golf 2014, 2019-20 | Driver Assistance | Stock | 0mph | 0mph | +| Volkswagen| Golf 2015-20 | Driver Assistance | Stock | 0mph | 0mph | +| Volkswagen| Golf Alltrack 2017-18 | Driver Assistance | Stock | 0mph | 0mph | +| Volkswagen| Golf GTE 2016 | Driver Assistance | Stock | 0mph | 0mph | +| Volkswagen| Golf GTI 2018-20 | Driver Assistance | Stock | 0mph | 0mph | +| Volkswagen| Golf R 2016-19 | Driver Assistance | Stock | 0mph | 0mph | +| Volkswagen| Golf SportsVan 2016 | Driver Assistance | Stock | 0mph | 0mph | +| Volkswagen| Golf SportWagen 2015 | Driver Assistance | Stock | 0mph | 0mph | +| Volkswagen| Jetta 2018-20 | Driver Assistance | Stock | 0mph | 0mph | +| Volkswagen| Jetta GLI 2021 | Driver Assistance | Stock | 0mph | 0mph | +| Volkswagen| Passat 2016-183 | Driver Assistance | Stock | 0mph | 0mph | +| Volkswagen| Polo 2020 | Driver Assistance | Stock | 0mph | 0mph | +| Volkswagen| T-Cross 20214 | Driver Assistance | Stock | 0mph | 0mph | +| Volkswagen| T-Roc 20214 | Driver Assistance | Stock | 0mph | 0mph | +| Volkswagen| Taos 20224 | Driver Assistance | Stock | 0mph | 0mph | +| Volkswagen| Tiguan 2020 | Driver Assistance | Stock | 0mph | 0mph | +| Volkswagen| Touran 2017 | Driver Assistance | Stock | 0mph | 0mph | 1Requires an [OBD-II car harness](https://comma.ai/shop/products/comma-car-harness) and [community built ASCM harness](https://github.com/commaai/openpilot/wiki/GM#hardware). ***NOTE: disconnecting the ASCM disables Automatic Emergency Braking (AEB).***
2Not including the China market Kamiq, which is based on the (currently) unsupported PQ34 platform.
diff --git a/launch_env.sh b/launch_env.sh index cb0a0572d06089..95e0f0c8ac0ee5 100755 --- a/launch_env.sh +++ b/launch_env.sh @@ -7,7 +7,7 @@ export OPENBLAS_NUM_THREADS=1 export VECLIB_MAXIMUM_THREADS=1 if [ -z "$REQUIRED_NEOS_VERSION" ]; then - export REQUIRED_NEOS_VERSION="18" + export REQUIRED_NEOS_VERSION="19.1" fi if [ -z "$AGNOS_VERSION" ]; then diff --git a/lfs-push.sh b/lfs-push.sh deleted file mode 100755 index 7e3c713b0cb216..00000000000000 --- a/lfs-push.sh +++ /dev/null @@ -1,2 +0,0 @@ -#!/usr/bin/env bash -git -c lfs.pushurl=ssh://git@gitlab.com/commaai/openpilot-lfs.git push diff --git a/opendbc b/opendbc index c15d80c683cf49..572ab7641bf88b 160000 --- a/opendbc +++ b/opendbc @@ -1 +1 @@ -Subproject commit c15d80c683cf498063debd9631c4e5d9c1abfbd0 +Subproject commit 572ab7641bf88b6b156d902c625ca8ad9a9a77be diff --git a/panda b/panda index f5857bc8fd3e1d..2d96ef02df0e59 160000 --- a/panda +++ b/panda @@ -1 +1 @@ -Subproject commit f5857bc8fd3e1d2c27df342df85e202c7e844911 +Subproject commit 2d96ef02df0e591244988739008e5d162da28cd0 diff --git a/pyextra/acados_template/acados_ocp_solver_pyx.pyx b/pyextra/acados_template/acados_ocp_solver_pyx.pyx index b950d880b08a01..9bcf5bb46a1913 100644 --- a/pyextra/acados_template/acados_ocp_solver_pyx.pyx +++ b/pyextra/acados_template/acados_ocp_solver_pyx.pyx @@ -417,8 +417,9 @@ cdef class AcadosOcpSolverFast: string_value = value_.encode('utf-8') acados_solver_common.ocp_nlp_solver_opts_set(self.nlp_config, self.nlp_opts, field, &string_value[0]) - raise Exception('AcadosOcpSolver.options_set() does not support field {}.'\ - '\n Possible values are {}.'.format(field_, ', '.join(int_fields + double_fields + string_fields))) + else: + raise Exception('AcadosOcpSolver.options_set() does not support field {}.'\ + '\n Possible values are {}.'.format(field_, ', '.join(int_fields + double_fields + string_fields))) def __del__(self): diff --git a/release/files_common b/release/files_common index 139b2700028519..795438c5b4f870 100644 --- a/release/files_common +++ b/release/files_common @@ -75,13 +75,15 @@ selfdrive/version.py selfdrive/__init__.py selfdrive/config.py -selfdrive/crash.py +selfdrive/sentry.py selfdrive/swaglog.py selfdrive/logmessaged.py selfdrive/tombstoned.py selfdrive/pandad.py selfdrive/updated.py selfdrive/rtshield.py +selfdrive/statsd.py + selfdrive/controls/lib/dynamic_gas.py selfdrive/athena/__init__.py @@ -93,6 +95,8 @@ selfdrive/boardd/.gitignore selfdrive/boardd/SConscript selfdrive/boardd/__init__.py selfdrive/boardd/boardd.cc +selfdrive/boardd/boardd.h +selfdrive/boardd/main.cc selfdrive/boardd/boardd.py selfdrive/boardd/boardd_api_impl.pyx selfdrive/boardd/can_list_to_can_capnp.cc @@ -203,6 +207,8 @@ selfdrive/common/version.h selfdrive/common/swaglog.h selfdrive/common/swaglog.cc +selfdrive/common/statlog.h +selfdrive/common/statlog.cc selfdrive/common/util.cc selfdrive/common/util.h selfdrive/common/queue.h @@ -233,17 +239,19 @@ selfdrive/controls/radard.py selfdrive/controls/lib/__init__.py selfdrive/controls/lib/alertmanager.py selfdrive/controls/lib/alerts_offroad.json -selfdrive/controls/lib/events.py +selfdrive/controls/lib/desire_helper.py selfdrive/controls/lib/drive_helpers.py -selfdrive/controls/lib/latcontrol_pid.py +selfdrive/controls/lib/events.py +selfdrive/controls/lib/lane_planner.py +selfdrive/controls/lib/latcontrol_angle.py selfdrive/controls/lib/latcontrol_indi.py selfdrive/controls/lib/latcontrol_lqr.py -selfdrive/controls/lib/latcontrol_angle.py -selfdrive/controls/lib/longcontrol.py +selfdrive/controls/lib/latcontrol_pid.py +selfdrive/controls/lib/latcontrol.py selfdrive/controls/lib/lateral_planner.py -selfdrive/controls/lib/lane_planner.py -selfdrive/controls/lib/pid.py +selfdrive/controls/lib/longcontrol.py selfdrive/controls/lib/longitudinal_planner.py +selfdrive/controls/lib/pid.py selfdrive/controls/lib/radar_helpers.py selfdrive/controls/lib/vehicle_model.py @@ -261,6 +269,7 @@ selfdrive/hardware/base.py selfdrive/hardware/hw.h selfdrive/hardware/eon/__init__.py selfdrive/hardware/eon/androidd.py +selfdrive/hardware/eon/shutdownd.py selfdrive/hardware/eon/hardware.h selfdrive/hardware/eon/hardware.py selfdrive/hardware/eon/neos.py @@ -555,7 +564,7 @@ opendbc/can/dbc_out/.gitignore opendbc/chrysler_pacifica_2017_hybrid.dbc opendbc/chrysler_pacifica_2017_hybrid_private_fusion.dbc -opendbc/gm_global_a_powertrain.dbc +opendbc/gm_global_a_powertrain_generated.dbc opendbc/gm_global_a_object.dbc opendbc/gm_global_a_chassis.dbc @@ -577,8 +586,6 @@ opendbc/honda_crv_hybrid_2019_can_generated.dbc opendbc/honda_fit_ex_2018_can_generated.dbc opendbc/honda_odyssey_exl_2018_generated.dbc opendbc/honda_odyssey_extreme_edition_2018_china_can_generated.dbc -opendbc/honda_pilot_touring_2017_can_generated.dbc -opendbc/honda_ridgeline_black_edition_2017_can_generated.dbc opendbc/honda_insight_ex_2019_can_generated.dbc opendbc/acura_ilx_2016_nidec.dbc @@ -595,23 +602,9 @@ opendbc/subaru_outback_2015_generated.dbc opendbc/subaru_outback_2019_generated.dbc opendbc/subaru_forester_2017_generated.dbc -opendbc/toyota_rav4_hybrid_2017_pt_generated.dbc -opendbc/toyota_rav4_2017_pt_generated.dbc -opendbc/toyota_prius_2017_pt_generated.dbc -opendbc/toyota_corolla_2017_pt_generated.dbc -opendbc/lexus_rx_350_2016_pt_generated.dbc -opendbc/lexus_rx_hybrid_2017_pt_generated.dbc +opendbc/toyota_tnga_k_pt_generated.dbc +opendbc/toyota_new_mc_pt_generated.dbc opendbc/toyota_nodsu_pt_generated.dbc -opendbc/toyota_nodsu_hybrid_pt_generated.dbc -opendbc/toyota_camry_hybrid_2018_pt_generated.dbc -opendbc/toyota_highlander_2017_pt_generated.dbc -opendbc/toyota_highlander_hybrid_2018_pt_generated.dbc -opendbc/toyota_avalon_2017_pt_generated.dbc -opendbc/toyota_sienna_xle_2018_pt_generated.dbc -opendbc/lexus_is_2018_pt_generated.dbc -opendbc/lexus_ct200h_2018_pt_generated.dbc -opendbc/lexus_nx300h_2018_pt_generated.dbc -opendbc/lexus_nx300_2018_pt_generated.dbc opendbc/toyota_adas.dbc opendbc/toyota_tss2_adas.dbc diff --git a/selfdrive/athena/athenad.py b/selfdrive/athena/athenad.py index a0f17397a4a400..275e3368467194 100755 --- a/selfdrive/athena/athenad.py +++ b/selfdrive/athena/athenad.py @@ -11,9 +11,10 @@ import socket import threading import time +import tempfile from collections import namedtuple from functools import partial -from typing import Any +from typing import Any, Dict import requests from jsonrpc import JSONRPCResponseManager, dispatcher @@ -31,6 +32,7 @@ from selfdrive.loggerd.xattr_cache import getxattr, setxattr from selfdrive.swaglog import cloudlog, SWAGLOG_DIR from selfdrive.version import get_version, get_origin, get_short_branch, get_commit +from selfdrive.statsd import STATS_DIR ATHENA_HOST = os.getenv('ATHENA_HOST', 'wss://athena.comma.ai') HANDLER_THREADS = int(os.getenv('HANDLER_THREADS', "4")) @@ -48,12 +50,12 @@ recv_queue: Any = queue.Queue() send_queue: Any = queue.Queue() upload_queue: Any = queue.Queue() -log_send_queue: Any = queue.Queue() +low_priority_send_queue: Any = queue.Queue() log_recv_queue: Any = queue.Queue() cancelled_uploads: Any = set() UploadItem = namedtuple('UploadItem', ['path', 'url', 'headers', 'created_at', 'id', 'retry_count', 'current', 'progress'], defaults=(0, False, 0)) -cur_upload_items = {} +cur_upload_items: Dict[int, Any] = {} class UploadQueueCache(): @@ -86,6 +88,7 @@ def handle_long_poll(ws): threading.Thread(target=ws_send, args=(ws, end_event), name='ws_send'), threading.Thread(target=upload_handler, args=(end_event,), name='upload_handler'), threading.Thread(target=log_handler, args=(end_event,), name='log_handler'), + threading.Thread(target=stat_handler, args=(end_event,), name='stat_handler'), ] + [ threading.Thread(target=jsonrpc_handler, args=(end_event,), name=f'worker_{x}') for x in range(HANDLER_THREADS) @@ -125,7 +128,26 @@ def jsonrpc_handler(end_event): send_queue.put_nowait(json.dumps({"error": str(e)})) -def upload_handler(end_event): +def retry_upload(tid: int, end_event: threading.Event) -> None: + if cur_upload_items[tid].retry_count < MAX_RETRY_COUNT: + item = cur_upload_items[tid] + item = item._replace( + retry_count=item.retry_count + 1, + progress=0, + current=False + ) + upload_queue.put_nowait(item) + UploadQueueCache.cache(upload_queue) + + cur_upload_items[tid] = None + + for _ in range(RETRY_DELAY): + time.sleep(1) + if end_event.is_set(): + break + + +def upload_handler(end_event: threading.Event) -> None: tid = threading.get_ident() while not end_event.is_set(): @@ -142,27 +164,15 @@ def upload_handler(end_event): def cb(sz, cur): cur_upload_items[tid] = cur_upload_items[tid]._replace(progress=cur / sz if sz else 1) - _do_upload(cur_upload_items[tid], cb) + response = _do_upload(cur_upload_items[tid], cb) + if response.status_code not in (200, 201, 403, 412): + cloudlog.warning(f"athena.upload_handler.retry {response.status_code} {cur_upload_items[tid]}") + retry_upload(tid, end_event) UploadQueueCache.cache(upload_queue) except (requests.exceptions.Timeout, requests.exceptions.ConnectionError, requests.exceptions.SSLError) as e: cloudlog.warning(f"athena.upload_handler.retry {e} {cur_upload_items[tid]}") - if cur_upload_items[tid].retry_count < MAX_RETRY_COUNT: - item = cur_upload_items[tid] - item = item._replace( - retry_count=item.retry_count + 1, - progress=0, - current=False - ) - upload_queue.put_nowait(item) - UploadQueueCache.cache(upload_queue) - - cur_upload_items[tid] = None - - for _ in range(RETRY_DELAY): - time.sleep(1) - if end_event.is_set(): - break + retry_upload(tid, end_event) except queue.Empty: pass @@ -447,7 +457,7 @@ def log_handler(end_event): "jsonrpc": "2.0", "id": log_entry } - log_send_queue.put_nowait(json.dumps(jsonrpc)) + low_priority_send_queue.put_nowait(json.dumps(jsonrpc)) curr_log = log_entry except OSError: pass # file could be deleted by log rotation @@ -478,6 +488,32 @@ def log_handler(end_event): cloudlog.exception("athena.log_handler.exception") +def stat_handler(end_event): + while not end_event.is_set(): + last_scan = 0 + curr_scan = sec_since_boot() + try: + if curr_scan - last_scan > 10: + stat_filenames = list(filter(lambda name: not name.startswith(tempfile.gettempprefix()), os.listdir(STATS_DIR))) + if len(stat_filenames) > 0: + stat_path = os.path.join(STATS_DIR, stat_filenames[0]) + with open(stat_path) as f: + jsonrpc = { + "method": "storeStats", + "params": { + "stats": f.read() + }, + "jsonrpc": "2.0", + "id": stat_filenames[0] + } + low_priority_send_queue.put_nowait(json.dumps(jsonrpc)) + os.remove(stat_path) + last_scan = curr_scan + except Exception: + cloudlog.exception("athena.stat_handler.exception") + time.sleep(0.1) + + def ws_proxy_recv(ws, local_sock, ssock, end_event, global_end_event): while not (end_event.is_set() or global_end_event.is_set()): try: @@ -550,7 +586,7 @@ def ws_send(ws, end_event): try: data = send_queue.get_nowait() except queue.Empty: - data = log_send_queue.get(timeout=1) + data = low_priority_send_queue.get(timeout=1) for i in range(0, len(data), WS_FRAME_SIZE): frame = data[i:i+WS_FRAME_SIZE] last = i + WS_FRAME_SIZE >= len(data) diff --git a/selfdrive/athena/registration.py b/selfdrive/athena/registration.py index f19540eb87d6a1..10f5d46f77f2ba 100755 --- a/selfdrive/athena/registration.py +++ b/selfdrive/athena/registration.py @@ -10,13 +10,18 @@ from common.spinner import Spinner from common.basedir import PERSIST from selfdrive.controls.lib.alertmanager import set_offroad_alert -from selfdrive.hardware import HARDWARE +from selfdrive.hardware import HARDWARE, PC from selfdrive.swaglog import cloudlog UNREGISTERED_DONGLE_ID = "UnregisteredDevice" +def is_registered_device() -> bool: + dongle = Params().get("DongleId", encoding='utf-8') + return dongle not in (None, UNREGISTERED_DONGLE_ID) + + def register(show_spinner=False) -> str: params = Params() params.put("SubscriberInfo", HARDWARE.get_subscriber_info()) @@ -86,7 +91,7 @@ def register(show_spinner=False) -> str: if dongle_id: params.put("DongleId", dongle_id) - set_offroad_alert("Offroad_UnofficialHardware", dongle_id == UNREGISTERED_DONGLE_ID) + set_offroad_alert("Offroad_UnofficialHardware", (dongle_id == UNREGISTERED_DONGLE_ID) and not PC) return dongle_id diff --git a/selfdrive/athena/tests/test_athenad.py b/selfdrive/athena/tests/test_athenad.py index 06d762c180904e..fb5936dde19fd7 100755 --- a/selfdrive/athena/tests/test_athenad.py +++ b/selfdrive/athena/tests/test_athenad.py @@ -166,6 +166,31 @@ def test_upload_handler(self, host): finally: end_event.set() + @with_http_server + @mock.patch('requests.put') + def test_upload_handler_retry(self, host, mock_put): + for status, retry in ((500, True), (412, False)): + mock_put.return_value.status_code = status + fn = os.path.join(athenad.ROOT, 'qlog.bz2') + Path(fn).touch() + item = athenad.UploadItem(path=fn, url=f"{host}/qlog.bz2", headers={}, created_at=int(time.time()*1000), id='') + + end_event = threading.Event() + thread = threading.Thread(target=athenad.upload_handler, args=(end_event,)) + thread.start() + + athenad.upload_queue.put_nowait(item) + try: + self.wait_for_upload() + time.sleep(0.1) + + self.assertEqual(athenad.upload_queue.qsize(), 1 if retry else 0) + finally: + end_event.set() + + if retry: + self.assertEqual(athenad.upload_queue.get().retry_count, 1) + def test_upload_handler_timeout(self): """When an upload times out or fails to connect it should be placed back in the queue""" fn = os.path.join(athenad.ROOT, 'qlog.bz2') diff --git a/selfdrive/boardd/SConscript b/selfdrive/boardd/SConscript index 07ded56e1b8a89..922107509af15b 100644 --- a/selfdrive/boardd/SConscript +++ b/selfdrive/boardd/SConscript @@ -1,7 +1,7 @@ Import('env', 'envCython', 'common', 'cereal', 'messaging') libs = ['usb-1.0', common, cereal, messaging, 'pthread', 'zmq', 'capnp', 'kj'] -env.Program('boardd', ['boardd.cc', 'panda.cc', 'pigeon.cc'], LIBS=libs) +env.Program('boardd', ['main.cc', 'boardd.cc', 'panda.cc', 'pigeon.cc'], LIBS=libs) env.Library('libcan_list_to_can_capnp', ['can_list_to_can_capnp.cc']) envCython.Program('boardd_api_impl.so', 'boardd_api_impl.pyx', LIBS=["can_list_to_can_capnp", 'capnp', 'kj'] + envCython["LIBS"]) diff --git a/selfdrive/boardd/boardd.cc b/selfdrive/boardd/boardd.cc index b844db86a0d89a..c86307722db046 100644 --- a/selfdrive/boardd/boardd.cc +++ b/selfdrive/boardd/boardd.cc @@ -1,3 +1,5 @@ +#include "selfdrive/boardd/boardd.h" + #include #include #include @@ -10,12 +12,12 @@ #include #include #include +#include #include #include #include #include #include -#include #include @@ -26,9 +28,7 @@ #include "selfdrive/common/timing.h" #include "selfdrive/common/util.h" #include "selfdrive/hardware/hw.h" -#include "selfdrive/locationd/ublox_msg.h" -#include "selfdrive/boardd/panda.h" #include "selfdrive/boardd/pigeon.h" // -- Multi-panda conventions -- @@ -161,7 +161,8 @@ bool safety_setter_thread(std::vector pandas) { int safety_param; auto safety_configs = car_params.getSafetyConfigs(); - for (uint32_t i=0; i i) { @@ -173,9 +174,8 @@ bool safety_setter_thread(std::vector pandas) { safety_param = 0; } - LOGW("panda %d: setting safety model: %d with param %d", i, (int)safety_model, safety_param); - - panda->set_unsafe_mode(1); // see safety_declarations.h for allowed values + LOGW("panda %d: setting safety model: %d, param: %d, unsafe mode: %d", i, (int)safety_model, safety_param, unsafe_mode); + panda->set_unsafe_mode(unsafe_mode); panda->set_safety_model(safety_model, safety_param); } @@ -304,34 +304,34 @@ bool send_panda_states(PubMaster *pm, const std::vector &pandas, bool s std::vector pandaStates; for (const auto& panda : pandas){ - health_t pandaState = panda->get_state(); + health_t health = panda->get_state(); if (spoofing_started) { - pandaState.ignition_line = 1; + health.ignition_line_pkt = 1; } - ignition_local |= ((pandaState.ignition_line != 0) || (pandaState.ignition_can != 0)); + ignition_local |= ((health.ignition_line_pkt != 0) || (health.ignition_can_pkt != 0)); - pandaStates.push_back(pandaState); + pandaStates.push_back(health); } - for (uint32_t i=0; iset_safety_model(cereal::CarParams::SafetyModel::NO_OUTPUT); } #ifndef __x86_64__ bool power_save_desired = !ignition_local && !pigeon_active; - if (pandaState.power_save_enabled != power_save_desired) { + if (health.power_save_enabled_pkt != power_save_desired) { panda->set_power_saving(power_save_desired); } // set safety mode to NO_OUTPUT when car is off. ELM327 is an alternative if we want to leverage athenad/connect - if (!ignition_local && (pandaState.safety_model != (uint8_t)(cereal::CarParams::SafetyModel::NO_OUTPUT))) { + if (!ignition_local && (health.safety_mode_pkt != (uint8_t)(cereal::CarParams::SafetyModel::NO_OUTPUT))) { panda->set_safety_model(cereal::CarParams::SafetyModel::NO_OUTPUT); } #endif @@ -341,25 +341,27 @@ bool send_panda_states(PubMaster *pm, const std::vector &pandas, bool s } auto ps = pss[i]; - ps.setUptime(pandaState.uptime); - ps.setIgnitionLine(pandaState.ignition_line); - ps.setIgnitionCan(pandaState.ignition_can); - ps.setControlsAllowed(pandaState.controls_allowed); - ps.setGasInterceptorDetected(pandaState.gas_interceptor_detected); - ps.setCanRxErrs(pandaState.can_rx_errs); - ps.setCanSendErrs(pandaState.can_send_errs); - ps.setCanFwdErrs(pandaState.can_fwd_errs); - ps.setGmlanSendErrs(pandaState.gmlan_send_errs); + ps.setUptime(health.uptime_pkt); + ps.setBlockedCnt(health.blocked_msg_cnt_pkt); + ps.setIgnitionLine(health.ignition_line_pkt); + ps.setIgnitionCan(health.ignition_can_pkt); + ps.setControlsAllowed(health.controls_allowed_pkt); + ps.setGasInterceptorDetected(health.gas_interceptor_detected_pkt); + ps.setCanRxErrs(health.can_rx_errs_pkt); + ps.setCanSendErrs(health.can_send_errs_pkt); + ps.setCanFwdErrs(health.can_fwd_errs_pkt); + ps.setGmlanSendErrs(health.gmlan_send_errs_pkt); ps.setPandaType(panda->hw_type); - ps.setSafetyModel(cereal::CarParams::SafetyModel(pandaState.safety_model)); - ps.setSafetyParam(pandaState.safety_param); - ps.setFaultStatus(cereal::PandaState::FaultStatus(pandaState.fault_status)); - ps.setPowerSaveEnabled((bool)(pandaState.power_save_enabled)); - ps.setHeartbeatLost((bool)(pandaState.heartbeat_lost)); - ps.setHarnessStatus(cereal::PandaState::HarnessStatus(pandaState.car_harness_status)); + ps.setSafetyModel(cereal::CarParams::SafetyModel(health.safety_mode_pkt)); + ps.setSafetyParam(health.safety_param_pkt); + ps.setFaultStatus(cereal::PandaState::FaultStatus(health.fault_status_pkt)); + ps.setPowerSaveEnabled((bool)(health.power_save_enabled_pkt)); + ps.setHeartbeatLost((bool)(health.heartbeat_lost_pkt)); + ps.setUnsafeMode(health.unsafe_mode_pkt); + ps.setHarnessStatus(cereal::PandaState::HarnessStatus(health.car_harness_status_pkt)); // Convert faults bitset to capnp list - std::bitset fault_bits(pandaState.faults); + std::bitset fault_bits(health.faults_pkt); auto faults = ps.initFaults(fault_bits.count()); size_t j = 0; @@ -396,12 +398,12 @@ void send_peripheral_state(PubMaster *pm, Panda *panda) { LOGW("reading hwmon took %lfms", read_time); } } else { - ps.setVoltage(pandaState.voltage); - ps.setCurrent(pandaState.current); + ps.setVoltage(pandaState.voltage_pkt); + ps.setCurrent(pandaState.current_pkt); } uint16_t fan_speed_rpm = panda->get_fan_speed(); - ps.setUsbPowerMode(cereal::PeripheralState::UsbPowerMode(pandaState.usb_power_mode)); + ps.setUsbPowerMode(cereal::PeripheralState::UsbPowerMode(pandaState.usb_power_mode_pkt)); ps.setFanSpeedRpm(fan_speed_rpm); pm->send("peripheralState", msg); @@ -421,6 +423,8 @@ void panda_state_thread(PubMaster *pm, std::vector pandas, bool spoofin // run at 2hz while (!do_exit && check_all_connected(pandas)) { + uint64_t start_time = nanos_since_boot(); + // send out peripheralState send_peripheral_state(pm, peripheral_panda); ignition = send_panda_states(pm, pandas, spoofing_started); @@ -455,7 +459,9 @@ void panda_state_thread(PubMaster *pm, std::vector pandas, bool spoofin // TODO do something cool with led // panda->set_red_led(sm["deviceState"].getDeviceState().getStartedSentry()); // spams red led } - util::sleep_for(500); + + uint64_t dt = nanos_since_boot() - start_time; + util::sleep_for(500 - dt / 1000000ULL); } } @@ -554,38 +560,11 @@ void pigeon_thread(Panda *panda) { std::unique_ptr pigeon(Hardware::TICI() ? Pigeon::connect("/dev/ttyHS0") : Pigeon::connect(panda)); - std::unordered_map last_recv_time; - std::unordered_map cls_max_dt = { - {(char)ublox::CLASS_NAV, int64_t(900000000ULL)}, // 0.9s - {(char)ublox::CLASS_RXM, int64_t(900000000ULL)}, // 0.9s - }; - while (!do_exit && panda->connected) { bool need_reset = false; bool ignition_local = ignition; std::string recv = pigeon->receive(); - // Parse message header - if (ignition_local && recv.length() >= 3) { - if (recv[0] == (char)ublox::PREAMBLE1 && recv[1] == (char)ublox::PREAMBLE2) { - const char msg_cls = recv[2]; - uint64_t t = nanos_since_boot(); - if (t > last_recv_time[msg_cls]) { - last_recv_time[msg_cls] = t; - } - } - } - - // Check based on message frequency - for (const auto& [msg_cls, max_dt] : cls_max_dt) { - int64_t dt = (int64_t)nanos_since_boot() - (int64_t)last_recv_time[msg_cls]; - if (ignition_last && ignition_local && dt > max_dt) { - LOGD("ublox receive timeout, msg class: 0x%02x, dt %llu", msg_cls, dt); - // TODO: turn on reset after verification of logs - // need_reset = true; - } - } - // Check based on null bytes if (ignition_local && recv.length() > 0 && recv[0] == (char)0x00) { need_reset = true; @@ -601,12 +580,6 @@ void pigeon_thread(Panda *panda) { if((ignition_local && !ignition_last) || need_reset) { pigeon_active = true; pigeon->init(); - - // Set receive times to current time - uint64_t t = nanos_since_boot() + 10000000000ULL; // Give ublox 10 seconds to start - for (const auto& [msg_cls, dt] : cls_max_dt) { - last_recv_time[msg_cls] = t; - } } else if (!ignition_local && ignition_last) { // power off on falling edge of ignition LOGD("powering off pigeon\n"); @@ -622,22 +595,11 @@ void pigeon_thread(Panda *panda) { } } -int main(int argc, char *argv[]) { - LOGW("starting boardd"); - - if (!Hardware::PC()) { - int err; - err = util::set_realtime_priority(54); - assert(err == 0); - err = util::set_core_affinity({Hardware::TICI() ? 4 : 3}); - assert(err == 0); - } +void boardd_main_thread(std::vector serials) { + if (serials.size() == 0) serials.push_back(""); - LOGW("attempting to connect"); PubMaster pm({"pandaStates", "peripheralState"}); - - std::vector serials(argv + 1, argv + argc); - if (serials.size() == 0) serials.push_back(""); + LOGW("attempting to connect"); // connect to all provided serials std::vector pandas; @@ -660,6 +622,8 @@ int main(int argc, char *argv[]) { Panda *peripheral_panda = pandas[0]; std::vector threads; + Params().put("LastPeripheralPandaType", std::to_string((int) peripheral_panda->get_hw_type())); + threads.emplace_back(panda_state_thread, &pm, pandas, getenv("STARTED") != nullptr); threads.emplace_back(peripheral_control_thread, peripheral_panda); threads.emplace_back(pigeon_thread, peripheral_panda); diff --git a/selfdrive/boardd/boardd.h b/selfdrive/boardd/boardd.h new file mode 100644 index 00000000000000..d3c9e1f94a8e2e --- /dev/null +++ b/selfdrive/boardd/boardd.h @@ -0,0 +1,6 @@ +#pragma once + +#include "selfdrive/boardd/panda.h" + +bool safety_setter_thread(std::vector pandas); +void boardd_main_thread(std::vector serials); diff --git a/selfdrive/boardd/main.cc b/selfdrive/boardd/main.cc new file mode 100644 index 00000000000000..d802e42f86ce5f --- /dev/null +++ b/selfdrive/boardd/main.cc @@ -0,0 +1,22 @@ +#include + +#include "selfdrive/boardd/boardd.h" +#include "selfdrive/common/swaglog.h" +#include "selfdrive/common/util.h" +#include "selfdrive/hardware/hw.h" + +int main(int argc, char *argv[]) { + LOGW("starting boardd"); + + if (!Hardware::PC()) { + int err; + err = util::set_realtime_priority(54); + assert(err == 0); + err = util::set_core_affinity({Hardware::TICI() ? 4 : 3}); + assert(err == 0); + } + + std::vector serials(argv + 1, argv + argc); + boardd_main_thread(serials); + return 0; +} diff --git a/selfdrive/boardd/panda.cc b/selfdrive/boardd/panda.cc index 6850ad41b26522..f38101f49fb8c5 100644 --- a/selfdrive/boardd/panda.cc +++ b/selfdrive/boardd/panda.cc @@ -45,11 +45,11 @@ Panda::Panda(std::string serial, uint32_t bus_offset) : bus_offset(bus_offset) { libusb_device_descriptor desc; libusb_get_device_descriptor(dev_list[i], &desc); if (desc.idVendor == 0xbbaa && desc.idProduct == 0xddcc) { - libusb_open(dev_list[i], &dev_handle); - if (dev_handle == NULL) { goto fail; } + int ret = libusb_open(dev_list[i], &dev_handle); + if (dev_handle == NULL || ret < 0) { goto fail; } unsigned char desc_serial[26] = { 0 }; - int ret = libusb_get_string_descriptor_ascii(dev_handle, desc.iSerialNumber, desc_serial, std::size(desc_serial)); + ret = libusb_get_string_descriptor_ascii(dev_handle, desc.iSerialNumber, desc_serial, std::size(desc_serial)); if (ret < 0) { goto fail; } usb_serial = std::string((char *)desc_serial, ret).c_str(); @@ -130,12 +130,14 @@ std::vector Panda::list() { libusb_get_device_descriptor(device, &desc); if (desc.idVendor == 0xbbaa && desc.idProduct == 0xddcc) { libusb_device_handle *handle = NULL; - libusb_open(device, &handle); + int ret = libusb_open(device, &handle); + if (ret < 0) { goto finish; } + unsigned char desc_serial[26] = { 0 }; - int ret = libusb_get_string_descriptor_ascii(handle, desc.iSerialNumber, desc_serial, std::size(desc_serial)); + ret = libusb_get_string_descriptor_ascii(handle, desc.iSerialNumber, desc_serial, std::size(desc_serial)); libusb_close(handle); - if (ret < 0) { goto finish; } + serials.push_back(std::string((char *)desc_serial, ret).c_str()); } } diff --git a/selfdrive/boardd/panda.h b/selfdrive/boardd/panda.h index 1a18a7f15a4a5e..0e88bb38501f65 100644 --- a/selfdrive/boardd/panda.h +++ b/selfdrive/boardd/panda.h @@ -13,6 +13,7 @@ #include "cereal/gen/cpp/car.capnp.h" #include "cereal/gen/cpp/log.capnp.h" +#include "panda/board/health.h" #define TIMEOUT 0 #define PANDA_BUS_CNT 4 @@ -24,29 +25,6 @@ #define CANPACKET_REJECTED (0xC0U) #define CANPACKET_RETURNED (0x80U) -// copied from panda/board/main.c -struct __attribute__((packed)) health_t { - uint32_t uptime; - uint32_t voltage; - uint32_t current; - uint32_t can_rx_errs; - uint32_t can_send_errs; - uint32_t can_fwd_errs; - uint32_t gmlan_send_errs; - uint32_t faults; - uint8_t ignition_line; - uint8_t ignition_can; - uint8_t controls_allowed; - uint8_t gas_interceptor_detected; - uint8_t car_harness_status; - uint8_t usb_power_mode; - uint8_t safety_model; - int16_t safety_param; - uint8_t fault_status; - uint8_t power_save_enabled; - uint8_t heartbeat_lost; -}; - struct __attribute__((packed)) can_header { uint8_t reserved : 1; uint8_t bus : 3; diff --git a/selfdrive/camerad/cameras/camera_common.cc b/selfdrive/camerad/cameras/camera_common.cc index 424ad78ab397d8..b313f74e7edf3a 100644 --- a/selfdrive/camerad/cameras/camera_common.cc +++ b/selfdrive/camerad/cameras/camera_common.cc @@ -19,6 +19,7 @@ #include "selfdrive/hardware/hw.h" #ifdef QCOM +#include "CL/cl_ext_qcom.h" #include "selfdrive/camerad/cameras/camera_qcom.h" #elif QCOM2 #include "selfdrive/camerad/cameras/camera_qcom2.h" @@ -28,6 +29,8 @@ #include "selfdrive/camerad/cameras/camera_replay.h" #endif +ExitHandler do_exit; + class Debayer { public: Debayer(cl_device_id device_id, cl_context context, const CameraBuf *b, const CameraState *s) { @@ -339,8 +342,6 @@ float set_exposure_target(const CameraBuf *b, int x_start, int x_end, int x_skip return lum_med / 256.0; } -extern ExitHandler do_exit; - void *processing_thread(MultiCameraState *cameras, CameraState *cs, process_thread_cb callback) { const char *thread_name = nullptr; if (cs == &cameras->road_cam) { @@ -422,3 +423,27 @@ void common_process_driver_camera(MultiCameraState *s, CameraState *c, int cnt) } s->pm->send("driverCameraState", msg); } + + +void camerad_thread() { + cl_device_id device_id = cl_get_device_id(CL_DEVICE_TYPE_DEFAULT); + // TODO: do this for QCOM2 too +#if defined(QCOM) + const cl_context_properties props[] = {CL_CONTEXT_PRIORITY_HINT_QCOM, CL_PRIORITY_HINT_HIGH_QCOM, 0}; + cl_context context = CL_CHECK_ERR(clCreateContext(props, 1, &device_id, NULL, NULL, &err)); +#else + cl_context context = CL_CHECK_ERR(clCreateContext(NULL, 1, &device_id, NULL, NULL, &err)); +#endif + + MultiCameraState cameras = {}; + VisionIpcServer vipc_server("camerad", device_id, context); + + cameras_init(&vipc_server, &cameras, device_id, context); + cameras_open(&cameras); + + vipc_server.start_listener(); + + cameras_run(&cameras); + + CL_CHECK(clReleaseContext(context)); +} diff --git a/selfdrive/camerad/cameras/camera_common.h b/selfdrive/camerad/cameras/camera_common.h index 75bd79bfdf0c82..f53f06dcebc599 100644 --- a/selfdrive/camerad/cameras/camera_common.h +++ b/selfdrive/camerad/cameras/camera_common.h @@ -41,6 +41,11 @@ const bool env_send_driver = getenv("SEND_DRIVER") != NULL; const bool env_send_road = getenv("SEND_ROAD") != NULL; const bool env_send_wide_road = getenv("SEND_WIDE_ROAD") != NULL; +// for debugging +// note: ONLY_ROAD doesn't work, likely due to a mixup with wideRoad cam in the kernel +const bool env_only_driver = getenv("ONLY_DRIVER") != NULL; +const bool env_debug_frames = getenv("DEBUG_FRAMES") != NULL; + typedef void (*release_cb)(void *cookie, int buf_idx); typedef struct CameraInfo { @@ -129,3 +134,4 @@ void cameras_open(MultiCameraState *s); void cameras_run(MultiCameraState *s); void cameras_close(MultiCameraState *s); void camera_autoexposure(CameraState *s, float grey_frac); +void camerad_thread(); diff --git a/selfdrive/camerad/cameras/camera_qcom2.cc b/selfdrive/camerad/cameras/camera_qcom2.cc index 030bbe47e1e15a..e86de9ffd62d12 100644 --- a/selfdrive/camerad/cameras/camera_qcom2.cc +++ b/selfdrive/camerad/cameras/camera_qcom2.cc @@ -60,7 +60,7 @@ int cam_control(int fd, int op_code, void *handle, int size) { struct cam_control camcontrol = {0}; camcontrol.op_code = op_code; camcontrol.handle = (uint64_t)handle; - if (size == 0) { + if (size == 0) { camcontrol.size = 8; camcontrol.handle_type = CAM_HANDLE_MEM_HANDLE; } else { @@ -227,7 +227,7 @@ void sensors_init(int video0_fd, int sensor_fd, int camera_num) { buf_desc[0].size = buf_desc[0].length = sizeof(struct cam_cmd_i2c_info) + sizeof(struct cam_cmd_probe); buf_desc[0].type = CAM_CMD_BUF_LEGACY; struct cam_cmd_i2c_info *i2c_info = (struct cam_cmd_i2c_info *)alloc_w_mmu_hdl(video0_fd, buf_desc[0].size, (uint32_t*)&buf_desc[0].mem_handle); - struct cam_cmd_probe *probe = (struct cam_cmd_probe *)((uint8_t *)i2c_info) + sizeof(struct cam_cmd_i2c_info); + auto probe = (struct cam_cmd_probe *)(i2c_info + 1); switch (camera_num) { case 0: @@ -353,6 +353,9 @@ void config_isp(struct CameraState *s, int io_mem_handle, int fence, int request struct cam_packet *pkt = (struct cam_packet *)alloc_w_mmu_hdl(s->multi_cam_state->video0_fd, size, &cam_packet_handle); pkt->num_cmd_buf = 2; pkt->kmd_cmd_buf_index = 0; + // YUV has kmd_cmd_buf_offset = 1780 + // I guess this is the ISP command + // YUV also has patch_offset = 0x1030 and num_patches = 10 if (io_mem_handle != 0) { pkt->io_configs_offset = sizeof(struct cam_cmd_buf_desc)*2; @@ -377,28 +380,65 @@ void config_isp(struct CameraState *s, int io_mem_handle, int fence, int request buf_desc[0].mem_handle = buf0_mem_handle; buf_desc[0].offset = buf0_offset; - // cam_isp_packet_generic_blob_handler - uint32_t tmp[] = { - // size is 0x20, type is 0(CAM_ISP_GENERIC_BLOB_TYPE_HFR_CONFIG) - 0x2000, - 0x1, 0x0, CAM_ISP_IFE_OUT_RES_RDI_0, 0x1, 0x0, 0x1, 0x0, 0x0, // 1 port, CAM_ISP_IFE_OUT_RES_RDI_0 - // size is 0x38, type is 1(CAM_ISP_GENERIC_BLOB_TYPE_CLOCK_CONFIG), clocks - 0x3801, - 0x1, 0x4, // Dual mode, 4 RDI wires - 0x18148d00, 0x0, 0x18148d00, 0x0, 0x18148d00, 0x0, // rdi clock - 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, // junk? - // offset 0x60 - // size is 0xe0, type is 2(CAM_ISP_GENERIC_BLOB_TYPE_BW_CONFIG), bandwidth - 0xe002, - 0x1, 0x4, // 4 RDI - 0x0, 0x0, 0x1ad27480, 0x0, 0x1ad27480, 0x0, // left_pix_vote - 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, // right_pix_vote - 0x0, 0x0, 0x6ee11c0, 0x2, 0x6ee11c0, 0x2, // rdi_vote - 0x0, 0x0, 0x0, 0x0, - 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, - 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, - 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, - 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0}; + // parsed by cam_isp_packet_generic_blob_handler + struct isp_packet { + uint32_t type_0; + cam_isp_resource_hfr_config resource_hfr; + + uint32_t type_1; + cam_isp_clock_config clock; + uint64_t extra_rdi_hz[3]; + + uint32_t type_2; + cam_isp_bw_config bw; + struct cam_isp_bw_vote extra_rdi_vote[6]; + } __attribute__((packed)) tmp; + memset(&tmp, 0, sizeof(tmp)); + + tmp.type_0 = CAM_ISP_GENERIC_BLOB_TYPE_HFR_CONFIG; + tmp.type_0 |= sizeof(cam_isp_resource_hfr_config) << 8; + static_assert(sizeof(cam_isp_resource_hfr_config) == 0x20); + tmp.resource_hfr = { + .num_ports = 1, // 10 for YUV (but I don't think we need them) + .port_hfr_config[0] = { + .resource_type = CAM_ISP_IFE_OUT_RES_RDI_0, // CAM_ISP_IFE_OUT_RES_FULL for YUV + .subsample_pattern = 1, + .subsample_period = 0, + .framedrop_pattern = 1, + .framedrop_period = 0, + }}; + + tmp.type_1 = CAM_ISP_GENERIC_BLOB_TYPE_CLOCK_CONFIG; + tmp.type_1 |= (sizeof(cam_isp_clock_config) + sizeof(tmp.extra_rdi_hz)) << 8; + static_assert((sizeof(cam_isp_clock_config) + sizeof(tmp.extra_rdi_hz)) == 0x38); + tmp.clock = { + .usage_type = 1, // dual mode + .num_rdi = 4, + .left_pix_hz = 404000000, + .right_pix_hz = 404000000, + .rdi_hz[0] = 404000000, + }; + + + tmp.type_2 = CAM_ISP_GENERIC_BLOB_TYPE_BW_CONFIG; + tmp.type_2 |= (sizeof(cam_isp_bw_config) + sizeof(tmp.extra_rdi_vote)) << 8; + static_assert((sizeof(cam_isp_bw_config) + sizeof(tmp.extra_rdi_vote)) == 0xe0); + tmp.bw = { + .usage_type = 1, // dual mode + .num_rdi = 4, + .left_pix_vote = { + .resource_id = 0, + .cam_bw_bps = 450000000, + .ext_bw_bps = 450000000, + }, + .rdi_vote[0] = { + .resource_id = 0, + .cam_bw_bps = 8706200000, + .ext_bw_bps = 8706200000, + }, + }; + + static_assert(offsetof(struct isp_packet, type_2) == 0x60); buf_desc[1].size = sizeof(tmp); buf_desc[1].offset = io_mem_handle != 0 ? 0x60 : 0; @@ -406,7 +446,7 @@ void config_isp(struct CameraState *s, int io_mem_handle, int fence, int request buf_desc[1].type = CAM_CMD_BUF_GENERIC; buf_desc[1].meta_data = CAM_ISP_PACKET_META_GENERIC_BLOB_COMMON; uint32_t *buf2 = (uint32_t *)alloc_w_mmu_hdl(s->multi_cam_state->video0_fd, buf_desc[1].size, (uint32_t*)&buf_desc[1].mem_handle, 0x20); - memcpy(buf2, tmp, sizeof(tmp)); + memcpy(buf2, &tmp, sizeof(tmp)); if (io_mem_handle != 0) { io_cfg[0].mem_handle[0] = io_mem_handle; @@ -415,19 +455,20 @@ void config_isp(struct CameraState *s, int io_mem_handle, int fence, int request .height = FRAME_HEIGHT, .plane_stride = FRAME_STRIDE, .slice_height = FRAME_HEIGHT, - .meta_stride = 0x0, + .meta_stride = 0x0, // YUV has meta(stride=0x400, size=0x5000) .meta_size = 0x0, .meta_offset = 0x0, - .packer_config = 0x0, - .mode_config = 0x0, + .packer_config = 0x0, // 0xb for YUV + .mode_config = 0x0, // 0x9ef for YUV .tile_config = 0x0, .h_init = 0x0, .v_init = 0x0, }; - io_cfg[0].format = CAM_FORMAT_MIPI_RAW_10; - io_cfg[0].color_pattern = 0x5; - io_cfg[0].bpp = 0xc; - io_cfg[0].resource_type = CAM_ISP_IFE_OUT_RES_RDI_0; + io_cfg[0].format = CAM_FORMAT_MIPI_RAW_10; // CAM_FORMAT_UBWC_TP10 for YUV + io_cfg[0].color_space = CAM_COLOR_SPACE_BASE; // CAM_COLOR_SPACE_BT601_FULL for YUV + io_cfg[0].color_pattern = 0x5; // 0x0 for YUV + io_cfg[0].bpp = 0xa; + io_cfg[0].resource_type = CAM_ISP_IFE_OUT_RES_RDI_0; // CAM_ISP_IFE_OUT_RES_FULL for YUV io_cfg[0].fence = fence; io_cfg[0].direction = CAM_BUF_OUTPUT; io_cfg[0].subsample_pattern = 0x1; @@ -553,18 +594,14 @@ int open_v4l_by_name_and_index(const char name[], int index, int flags = O_RDWR static void camera_open(CameraState *s) { s->sensor_fd = open_v4l_by_name_and_index("cam-sensor-driver", s->camera_num); assert(s->sensor_fd >= 0); - LOGD("opened sensor"); - - s->csiphy_fd = open_v4l_by_name_and_index("cam-csiphy-driver", s->camera_num); - assert(s->csiphy_fd >= 0); - LOGD("opened csiphy"); + LOGD("opened sensor for %d", s->camera_num); // probe the sensor LOGD("-- Probing sensor %d", s->camera_num); sensors_init(s->multi_cam_state->video0_fd, s->sensor_fd, s->camera_num); // create session - struct cam_req_mgr_session_info session_info = {}; + struct cam_req_mgr_session_info session_info = {}; int ret = cam_control(s->multi_cam_state->video0_fd, CAM_REQ_MGR_CREATE_SESSION, &session_info, sizeof(session_info)); LOGD("get session: %d 0x%X", ret, session_info.session_hdl); s->session_handle = session_info.session_hdl; @@ -605,7 +642,7 @@ static void camera_open(CameraState *s) { .pixel_clk = 0x0, .batch_size = 0x0, - .dsp_mode = 0x0, + .dsp_mode = CAM_ISP_DSP_MODE_NONE, .hbi_cnt = 0x0, .custom_csid = 0x0, @@ -627,9 +664,13 @@ static void camera_open(CameraState *s) { auto isp_dev_handle = device_acquire(s->multi_cam_state->isp_fd, s->session_handle, &isp_resource); assert(isp_dev_handle); - s->isp_dev_handle = *isp_dev_handle; + s->isp_dev_handle = *isp_dev_handle; LOGD("acquire isp dev"); + s->csiphy_fd = open_v4l_by_name_and_index("cam-csiphy-driver", s->camera_num); + assert(s->csiphy_fd >= 0); + LOGD("opened csiphy for %d", s->camera_num); + struct cam_csiphy_acquire_dev_info csiphy_acquire_dev_info = {.combo_mode = 0}; auto csiphy_dev_handle = device_acquire(s->csiphy_fd, s->session_handle, &csiphy_acquire_dev_info); assert(csiphy_dev_handle); @@ -645,6 +686,7 @@ static void camera_open(CameraState *s) { //sensors_i2c(s, start_reg_array, std::size(start_reg_array), CAM_SENSOR_PACKET_OPCODE_SENSOR_STREAMON); //sensors_i2c(s, stop_reg_array, std::size(stop_reg_array), CAM_SENSOR_PACKET_OPCODE_SENSOR_STREAMOFF); + // config csiphy LOG("-- Config CSI PHY"); { @@ -686,8 +728,8 @@ static void camera_open(CameraState *s) { req_mgr_link_info.dev_hdls[0] = s->isp_dev_handle; req_mgr_link_info.dev_hdls[1] = s->sensor_dev_handle; ret = cam_control(s->multi_cam_state->video0_fd, CAM_REQ_MGR_LINK, &req_mgr_link_info, sizeof(req_mgr_link_info)); - LOGD("link: %d", ret); s->link_handle = req_mgr_link_info.link_hdl; + LOGD("link: %d hdl: 0x%X", ret, s->link_handle); struct cam_req_mgr_link_control req_mgr_link_control = {0}; req_mgr_link_control.ops = CAM_REQ_MGR_LINK_ACTIVATE; @@ -708,15 +750,17 @@ static void camera_open(CameraState *s) { } void cameras_init(VisionIpcServer *v, MultiCameraState *s, cl_device_id device_id, cl_context ctx) { - camera_init(s, v, &s->road_cam, CAMERA_ID_AR0231, 1, 20, device_id, ctx, - VISION_STREAM_RGB_BACK, VISION_STREAM_ROAD); // swap left/right - printf("road camera initted \n"); - camera_init(s, v, &s->wide_road_cam, CAMERA_ID_AR0231, 0, 20, device_id, ctx, - VISION_STREAM_RGB_WIDE, VISION_STREAM_WIDE_ROAD); - printf("wide road camera initted \n"); camera_init(s, v, &s->driver_cam, CAMERA_ID_AR0231, 2, 20, device_id, ctx, VISION_STREAM_RGB_FRONT, VISION_STREAM_DRIVER); printf("driver camera initted \n"); + if (!env_only_driver) { + camera_init(s, v, &s->road_cam, CAMERA_ID_AR0231, 1, 20, device_id, ctx, + VISION_STREAM_RGB_BACK, VISION_STREAM_ROAD); // swap left/right + printf("road camera initted \n"); + camera_init(s, v, &s->wide_road_cam, CAMERA_ID_AR0231, 0, 20, device_id, ctx, + VISION_STREAM_RGB_WIDE, VISION_STREAM_WIDE_ROAD); + printf("wide road camera initted \n"); + } s->sm = new SubMaster({"driverState"}); s->pm = new PubMaster({"roadCameraState", "driverCameraState", "wideRoadCameraState", "thumbnail"}); @@ -763,12 +807,14 @@ void cameras_open(MultiCameraState *s) { ret = HANDLE_EINTR(ioctl(s->video0_fd, VIDIOC_SUBSCRIBE_EVENT, &sub)); printf("req mgr subscribe: %d\n", ret); - camera_open(&s->road_cam); - printf("road camera opened \n"); - camera_open(&s->wide_road_cam); - printf("wide road camera opened \n"); camera_open(&s->driver_cam); printf("driver camera opened \n"); + if (!env_only_driver) { + camera_open(&s->road_cam); + printf("road camera opened \n"); + camera_open(&s->wide_road_cam); + printf("wide road camera opened \n"); + } } static void camera_close(CameraState *s) { @@ -816,9 +862,11 @@ static void camera_close(CameraState *s) { } void cameras_close(MultiCameraState *s) { - camera_close(&s->road_cam); - camera_close(&s->wide_road_cam); camera_close(&s->driver_cam); + if (!env_only_driver) { + camera_close(&s->road_cam); + camera_close(&s->wide_road_cam); + } delete s->sm; delete s->pm; @@ -1010,16 +1058,20 @@ void process_road_camera(MultiCameraState *s, CameraState *c, int cnt) { void cameras_run(MultiCameraState *s) { LOG("-- Starting threads"); std::vector threads; - threads.push_back(start_process_thread(s, &s->road_cam, process_road_camera)); threads.push_back(start_process_thread(s, &s->driver_cam, common_process_driver_camera)); - threads.push_back(start_process_thread(s, &s->wide_road_cam, process_road_camera)); + if (!env_only_driver) { + threads.push_back(start_process_thread(s, &s->road_cam, process_road_camera)); + threads.push_back(start_process_thread(s, &s->wide_road_cam, process_road_camera)); + } // start devices LOG("-- Starting devices"); int start_reg_len = sizeof(start_reg_array) / sizeof(struct i2c_random_wr_payload); - sensors_i2c(&s->road_cam, start_reg_array, start_reg_len, CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG); - sensors_i2c(&s->wide_road_cam, start_reg_array, start_reg_len, CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG); sensors_i2c(&s->driver_cam, start_reg_array, start_reg_len, CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG); + if (!env_only_driver) { + sensors_i2c(&s->road_cam, start_reg_array, start_reg_len, CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG); + sensors_i2c(&s->wide_road_cam, start_reg_array, start_reg_len, CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG); + } // poll events LOG("-- Dequeueing Video events"); @@ -1043,8 +1095,10 @@ void cameras_run(MultiCameraState *s) { if (ret == 0) { if (ev.type == V4L_EVENT_CAM_REQ_MGR_EVENT) { struct cam_req_mgr_message *event_data = (struct cam_req_mgr_message *)ev.u.data; - // LOGD("v4l2 event: sess_hdl %d, link_hdl %d, frame_id %d, req_id %lld, timestamp 0x%llx, sof_status %d\n", event_data->session_hdl, event_data->u.frame_msg.link_hdl, event_data->u.frame_msg.frame_id, event_data->u.frame_msg.request_id, event_data->u.frame_msg.timestamp, event_data->u.frame_msg.sof_status); - // printf("sess_hdl %d, link_hdl %d, frame_id %lu, req_id %lu, timestamp 0x%lx, sof_status %d\n", event_data->session_hdl, event_data->u.frame_msg.link_hdl, event_data->u.frame_msg.frame_id, event_data->u.frame_msg.request_id, event_data->u.frame_msg.timestamp, event_data->u.frame_msg.sof_status); + // LOGD("v4l2 event: sess_hdl 0x%X, link_hdl 0x%X, frame_id %d, req_id %lld, timestamp 0x%llx, sof_status %d\n", event_data->session_hdl, event_data->u.frame_msg.link_hdl, event_data->u.frame_msg.frame_id, event_data->u.frame_msg.request_id, event_data->u.frame_msg.timestamp, event_data->u.frame_msg.sof_status); + if (env_debug_frames) { + printf("sess_hdl 0x%X, link_hdl 0x%X, frame_id %lu, req_id %lu, timestamp 0x%lx, sof_status %d\n", event_data->session_hdl, event_data->u.frame_msg.link_hdl, event_data->u.frame_msg.frame_id, event_data->u.frame_msg.request_id, event_data->u.frame_msg.timestamp, event_data->u.frame_msg.sof_status); + } if (event_data->session_hdl == s->road_cam.session_handle) { handle_camera_event(&s->road_cam, event_data); diff --git a/selfdrive/camerad/main.cc b/selfdrive/camerad/main.cc index f06bd341dfa74e..668410d6f7c2ae 100644 --- a/selfdrive/camerad/main.cc +++ b/selfdrive/camerad/main.cc @@ -1,48 +1,11 @@ -#include -#include -#include +#include "selfdrive/camerad/cameras/camera_common.h" #include -#include -#include -#include "libyuv.h" - -#include "cereal/visionipc/visionipc_server.h" -#include "selfdrive/common/clutil.h" #include "selfdrive/common/params.h" -#include "selfdrive/common/swaglog.h" #include "selfdrive/common/util.h" #include "selfdrive/hardware/hw.h" -#ifdef QCOM -#include "selfdrive/camerad/cameras/camera_qcom.h" -#elif QCOM2 -#include "selfdrive/camerad/cameras/camera_qcom2.h" -#elif WEBCAM -#include "selfdrive/camerad/cameras/camera_webcam.h" -#else -#include "selfdrive/camerad/cameras/camera_replay.h" -#endif - -ExitHandler do_exit; - -void party(cl_device_id device_id, cl_context context) { - MultiCameraState cameras = {}; - VisionIpcServer vipc_server("camerad", device_id, context); - - cameras_init(&vipc_server, &cameras, device_id, context); - cameras_open(&cameras); - - vipc_server.start_listener(); - - cameras_run(&cameras); -} - -#ifdef QCOM -#include "CL/cl_ext_qcom.h" -#endif - int main(int argc, char *argv[]) { if (!Hardware::PC()) { int ret; @@ -52,17 +15,6 @@ int main(int argc, char *argv[]) { assert(ret == 0 || Params().getBool("IsOffroad")); // failure ok while offroad due to offlining cores } - cl_device_id device_id = cl_get_device_id(CL_DEVICE_TYPE_DEFAULT); - - // TODO: do this for QCOM2 too -#if defined(QCOM) - const cl_context_properties props[] = {CL_CONTEXT_PRIORITY_HINT_QCOM, CL_PRIORITY_HINT_HIGH_QCOM, 0}; - cl_context context = CL_CHECK_ERR(clCreateContext(props, 1, &device_id, NULL, NULL, &err)); -#else - cl_context context = CL_CHECK_ERR(clCreateContext(NULL, 1, &device_id, NULL, NULL, &err)); -#endif - - party(device_id, context); - - CL_CHECK(clReleaseContext(context)); + camerad_thread(); + return 0; } diff --git a/selfdrive/camerad/test/ae_gray_test.cc b/selfdrive/camerad/test/ae_gray_test.cc index 0f14a23794c52c..358d93d75940b8 100644 --- a/selfdrive/camerad/test/ae_gray_test.cc +++ b/selfdrive/camerad/test/ae_gray_test.cc @@ -11,9 +11,10 @@ #include "selfdrive/camerad/cameras/camera_common.h" // needed by camera_common.cc -ExitHandler do_exit; - void camera_autoexposure(CameraState *s, float grey_frac) {} +void cameras_init(VisionIpcServer *v, MultiCameraState *s, cl_device_id device_id, cl_context ctx) {} +void cameras_open(MultiCameraState *s) {} +void cameras_run(MultiCameraState *s) {} int main() { // set up fake camerabuf diff --git a/selfdrive/car/car_helpers.py b/selfdrive/car/car_helpers.py index 1be6d0965571f0..89c9efbf4700ff 100644 --- a/selfdrive/car/car_helpers.py +++ b/selfdrive/car/car_helpers.py @@ -129,13 +129,13 @@ def fingerprint(logcan, sendcan): # The fingerprint dict is generated for all buses, this way the car interface # can use it to detect a (valid) multipanda setup and initialize accordingly if can.src < 128: - if can.src not in finger.keys(): + if can.src not in finger: finger[can.src] = {} finger[can.src][can.address] = len(can.dat) for b in candidate_cars: # Ignore extended messages and VIN query response. - if can.src == b and can.address < 0x800 and can.address not in [0x7df, 0x7e0, 0x7e8]: + if can.src == b and can.address < 0x800 and can.address not in (0x7df, 0x7e0, 0x7e8): candidate_cars[b] = eliminate_incompatible_cars(can, candidate_cars[b]) # if we only have one car choice and the time since we got our first diff --git a/selfdrive/car/chrysler/carstate.py b/selfdrive/car/chrysler/carstate.py index ab3125b011255b..30a0bf6a3a3f09 100644 --- a/selfdrive/car/chrysler/carstate.py +++ b/selfdrive/car/chrysler/carstate.py @@ -52,7 +52,7 @@ def update(self, cp, cp_cam): ret.cruiseState.available = ret.cruiseState.enabled # FIXME: for now same as enabled ret.cruiseState.speed = cp.vl["DASHBOARD"]["ACC_SPEED_CONFIG_KPH"] * CV.KPH_TO_MS # CRUISE_STATE is a three bit msg, 0 is off, 1 and 2 are Non-ACC mode, 3 and 4 are ACC mode, find if there are other states too - ret.cruiseState.nonAdaptive = cp.vl["DASHBOARD"]["CRUISE_STATE"] in [1, 2] + ret.cruiseState.nonAdaptive = cp.vl["DASHBOARD"]["CRUISE_STATE"] in (1, 2) ret.steeringTorque = cp.vl["EPS_STATUS"]["TORQUE_DRIVER"] ret.steeringTorqueEps = cp.vl["EPS_STATUS"]["TORQUE_MOTOR"] @@ -75,33 +75,33 @@ def update(self, cp, cp_cam): @staticmethod def get_can_parser(CP): signals = [ - # sig_name, sig_address, default - ("PRNDL", "GEAR", 0), - ("DOOR_OPEN_FL", "DOORS", 0), - ("DOOR_OPEN_FR", "DOORS", 0), - ("DOOR_OPEN_RL", "DOORS", 0), - ("DOOR_OPEN_RR", "DOORS", 0), - ("BRAKE_PRESSED_2", "BRAKE_2", 0), - ("ACCEL_134", "ACCEL_GAS_134", 0), - ("SPEED_LEFT", "SPEED_1", 0), - ("SPEED_RIGHT", "SPEED_1", 0), - ("WHEEL_SPEED_FL", "WHEEL_SPEEDS", 0), - ("WHEEL_SPEED_RR", "WHEEL_SPEEDS", 0), - ("WHEEL_SPEED_RL", "WHEEL_SPEEDS", 0), - ("WHEEL_SPEED_FR", "WHEEL_SPEEDS", 0), - ("STEER_ANGLE", "STEERING", 0), - ("STEERING_RATE", "STEERING", 0), - ("TURN_SIGNALS", "STEERING_LEVERS", 0), - ("ACC_STATUS_2", "ACC_2", 0), - ("HIGH_BEAM_FLASH", "STEERING_LEVERS", 0), - ("ACC_SPEED_CONFIG_KPH", "DASHBOARD", 0), - ("CRUISE_STATE", "DASHBOARD", 0), - ("TORQUE_DRIVER", "EPS_STATUS", 0), - ("TORQUE_MOTOR", "EPS_STATUS", 0), - ("LKAS_STATE", "EPS_STATUS", 1), - ("COUNTER", "EPS_STATUS", -1), - ("TRACTION_OFF", "TRACTION_BUTTON", 0), - ("SEATBELT_DRIVER_UNLATCHED", "SEATBELT_STATUS", 0), + # sig_name, sig_address + ("PRNDL", "GEAR"), + ("DOOR_OPEN_FL", "DOORS"), + ("DOOR_OPEN_FR", "DOORS"), + ("DOOR_OPEN_RL", "DOORS"), + ("DOOR_OPEN_RR", "DOORS"), + ("BRAKE_PRESSED_2", "BRAKE_2"), + ("ACCEL_134", "ACCEL_GAS_134"), + ("SPEED_LEFT", "SPEED_1"), + ("SPEED_RIGHT", "SPEED_1"), + ("WHEEL_SPEED_FL", "WHEEL_SPEEDS"), + ("WHEEL_SPEED_RR", "WHEEL_SPEEDS"), + ("WHEEL_SPEED_RL", "WHEEL_SPEEDS"), + ("WHEEL_SPEED_FR", "WHEEL_SPEEDS"), + ("STEER_ANGLE", "STEERING"), + ("STEERING_RATE", "STEERING"), + ("TURN_SIGNALS", "STEERING_LEVERS"), + ("ACC_STATUS_2", "ACC_2"), + ("HIGH_BEAM_FLASH", "STEERING_LEVERS"), + ("ACC_SPEED_CONFIG_KPH", "DASHBOARD"), + ("CRUISE_STATE", "DASHBOARD"), + ("TORQUE_DRIVER", "EPS_STATUS"), + ("TORQUE_MOTOR", "EPS_STATUS"), + ("LKAS_STATE", "EPS_STATUS"), + ("COUNTER", "EPS_STATUS",), + ("TRACTION_OFF", "TRACTION_BUTTON"), + ("SEATBELT_DRIVER_UNLATCHED", "SEATBELT_STATUS"), ] checks = [ @@ -123,20 +123,20 @@ def get_can_parser(CP): if CP.enableBsm: signals += [ - ("BLIND_SPOT_RIGHT", "BLIND_SPOT_WARNINGS", 0), - ("BLIND_SPOT_LEFT", "BLIND_SPOT_WARNINGS", 0), + ("BLIND_SPOT_RIGHT", "BLIND_SPOT_WARNINGS"), + ("BLIND_SPOT_LEFT", "BLIND_SPOT_WARNINGS"), ] - checks += [("BLIND_SPOT_WARNINGS", 2)] + checks.append(("BLIND_SPOT_WARNINGS", 2)) return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 0) @staticmethod def get_cam_can_parser(CP): signals = [ - # sig_name, sig_address, default - ("COUNTER", "LKAS_COMMAND", -1), - ("CAR_MODEL", "LKAS_HUD", -1), - ("LKAS_STATUS_OK", "LKAS_HEARTBIT", -1) + # sig_name, sig_address + ("COUNTER", "LKAS_COMMAND"), + ("CAR_MODEL", "LKAS_HUD"), + ("LKAS_STATUS_OK", "LKAS_HEARTBIT") ] checks = [ ("LKAS_COMMAND", 100), diff --git a/selfdrive/car/chrysler/chryslercan.py b/selfdrive/car/chrysler/chryslercan.py index c72f155298c6a5..4d5226570c521f 100644 --- a/selfdrive/car/chrysler/chryslercan.py +++ b/selfdrive/car/chrysler/chryslercan.py @@ -8,7 +8,7 @@ def create_lkas_hud(packer, gear, lkas_active, hud_alert, hud_count, lkas_car_model): # LKAS_HUD 0x2a6 (678) Controls what lane-keeping icon is displayed. - if hud_alert in [VisualAlert.steerRequired, VisualAlert.ldw]: + if hud_alert in (VisualAlert.steerRequired, VisualAlert.ldw): msg = b'\x00\x00\x00\x03\x00\x00\x00\x00' return make_can_msg(0x2a6, msg, 0) diff --git a/selfdrive/car/chrysler/interface.py b/selfdrive/car/chrysler/interface.py index a893b222f8b0bc..aead037815b7b6 100755 --- a/selfdrive/car/chrysler/interface.py +++ b/selfdrive/car/chrysler/interface.py @@ -60,8 +60,7 @@ def update(self, c, can_strings): ret.steeringRateLimited = self.CC.steer_rate_limited if self.CC is not None else False # events - events = self.create_common_events(ret, extra_gears=[car.CarState.GearShifter.low], - gas_resume_speed=2.) + events = self.create_common_events(ret, extra_gears=[car.CarState.GearShifter.low]) if ret.vEgo < self.CP.minSteerSpeed: events.add(car.CarEvent.EventName.belowSteerSpeed) diff --git a/selfdrive/car/chrysler/radar_interface.py b/selfdrive/car/chrysler/radar_interface.py index 3139efad34ab4d..5b8dba67f4c312 100755 --- a/selfdrive/car/chrysler/radar_interface.py +++ b/selfdrive/car/chrysler/radar_interface.py @@ -11,30 +11,25 @@ def _create_radar_can_parser(car_fingerprint): msg_n = len(RADAR_MSGS_C) - # list of [(signal name, message name or number, initial values), (...)] - # [('RADAR_STATE', 1024, 0), - # ('LONG_DIST', 1072, 255), - # ('LONG_DIST', 1073, 255), - # ('LONG_DIST', 1074, 255), - # ('LONG_DIST', 1075, 255), + # list of [(signal name, message name or number), (...)] + # [('RADAR_STATE', 1024), + # ('LONG_DIST', 1072), + # ('LONG_DIST', 1073), + # ('LONG_DIST', 1074), + # ('LONG_DIST', 1075), - # The factor and offset are applied by the dbc parsing library, so the - # default values should be after the factor/offset are applied. signals = list(zip(['LONG_DIST'] * msg_n + - ['LAT_DIST'] * msg_n + - ['REL_SPEED'] * msg_n, - RADAR_MSGS_C * 2 + # LONG_DIST, LAT_DIST - RADAR_MSGS_D, # REL_SPEED - [0] * msg_n + # LONG_DIST - [-1000] * msg_n + # LAT_DIST - [-146.278] * msg_n)) # REL_SPEED set to 0, factor/offset to this + ['LAT_DIST'] * msg_n + + ['REL_SPEED'] * msg_n, + RADAR_MSGS_C * 2 + # LONG_DIST, LAT_DIST + RADAR_MSGS_D)) # REL_SPEED # TODO what are the checks actually used for? # honda only checks the last message, # toyota checks all the messages. Which do we want? checks = list(zip(RADAR_MSGS_C + - RADAR_MSGS_D, - [20]*msg_n + # 20Hz (0.05s) - [20]*msg_n)) # 20Hz (0.05s) + RADAR_MSGS_D, + [20] * msg_n + # 20Hz (0.05s) + [20] * msg_n)) # 20Hz (0.05s) return CANParser(DBC[car_fingerprint]['radar'], signals, checks, 1) diff --git a/selfdrive/car/chrysler/values.py b/selfdrive/car/chrysler/values.py index e32de195017261..3fd32e9bc7659c 100644 --- a/selfdrive/car/chrysler/values.py +++ b/selfdrive/car/chrysler/values.py @@ -1,5 +1,3 @@ -# flake8: noqa - from selfdrive.car import dbc_dict from cereal import car Ecu = car.CarParams.Ecu diff --git a/selfdrive/car/ford/carcontroller.py b/selfdrive/car/ford/carcontroller.py index 6257cfd2095dea..389d3d8d8bffb6 100644 --- a/selfdrive/car/ford/carcontroller.py +++ b/selfdrive/car/ford/carcontroller.py @@ -22,7 +22,7 @@ def __init__(self, dbc_name, CP, VM): def update(self, enabled, CS, frame, actuators, visual_alert, pcm_cancel): can_sends = [] - steer_alert = visual_alert in [VisualAlert.steerRequired, VisualAlert.ldw] + steer_alert = visual_alert in (VisualAlert.steerRequired, VisualAlert.ldw) apply_steer = actuators.steer diff --git a/selfdrive/car/ford/carstate.py b/selfdrive/car/ford/carstate.py index a621ab16ab5c5c..eba623f5ce215a 100644 --- a/selfdrive/car/ford/carstate.py +++ b/selfdrive/car/ford/carstate.py @@ -25,7 +25,7 @@ def update(self, cp): ret.steeringPressed = not cp.vl["Lane_Keep_Assist_Status"]["LaHandsOff_B_Actl"] ret.steerError = cp.vl["Lane_Keep_Assist_Status"]["LaActDeny_B_Actl"] == 1 ret.cruiseState.speed = cp.vl["Cruise_Status"]["Set_Speed"] * CV.MPH_TO_MS - ret.cruiseState.enabled = not (cp.vl["Cruise_Status"]["Cruise_State"] in [0, 3]) + ret.cruiseState.enabled = not (cp.vl["Cruise_Status"]["Cruise_State"] in (0, 3)) ret.cruiseState.available = cp.vl["Cruise_Status"]["Cruise_State"] != 0 ret.gas = cp.vl["EngineData_14"]["ApedPosScal_Pc_Actl"] / 100. ret.gasPressed = ret.gas > 1e-6 @@ -39,20 +39,20 @@ def update(self, cp): @staticmethod def get_can_parser(CP): signals = [ - # sig_name, sig_address, default - ("WhlRr_W_Meas", "WheelSpeed_CG1", 0.), - ("WhlRl_W_Meas", "WheelSpeed_CG1", 0.), - ("WhlFr_W_Meas", "WheelSpeed_CG1", 0.), - ("WhlFl_W_Meas", "WheelSpeed_CG1", 0.), - ("SteWhlRelInit_An_Sns", "Steering_Wheel_Data_CG1", 0.), - ("Cruise_State", "Cruise_Status", 0.), - ("Set_Speed", "Cruise_Status", 0.), - ("LaActAvail_D_Actl", "Lane_Keep_Assist_Status", 0), - ("LaHandsOff_B_Actl", "Lane_Keep_Assist_Status", 0), - ("LaActDeny_B_Actl", "Lane_Keep_Assist_Status", 0), - ("ApedPosScal_Pc_Actl", "EngineData_14", 0.), - ("Dist_Incr", "Steering_Buttons", 0.), - ("Brake_Drv_Appl", "Cruise_Status", 0.), + # sig_name, sig_address + ("WhlRr_W_Meas", "WheelSpeed_CG1"), + ("WhlRl_W_Meas", "WheelSpeed_CG1"), + ("WhlFr_W_Meas", "WheelSpeed_CG1"), + ("WhlFl_W_Meas", "WheelSpeed_CG1"), + ("SteWhlRelInit_An_Sns", "Steering_Wheel_Data_CG1"), + ("Cruise_State", "Cruise_Status"), + ("Set_Speed", "Cruise_Status"), + ("LaActAvail_D_Actl", "Lane_Keep_Assist_Status"), + ("LaHandsOff_B_Actl", "Lane_Keep_Assist_Status"), + ("LaActDeny_B_Actl", "Lane_Keep_Assist_Status"), + ("ApedPosScal_Pc_Actl", "EngineData_14"), + ("Dist_Incr", "Steering_Buttons"), + ("Brake_Drv_Appl", "Cruise_Status"), ] checks = [] return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 0, enforce_checks=False) diff --git a/selfdrive/car/ford/fordcan.py b/selfdrive/car/ford/fordcan.py index e98dec584baa74..5a8b0b2dec5d79 100644 --- a/selfdrive/car/ford/fordcan.py +++ b/selfdrive/car/ford/fordcan.py @@ -6,7 +6,7 @@ def create_steer_command(packer, angle_cmd, enabled, lkas_state, angle_steers, c """Creates a CAN message for the Ford Steer Command.""" #if enabled and lkas available: - if enabled and lkas_state in [2, 3]: # and (frame % 500) >= 3: + if enabled and lkas_state in (2, 3): # and (frame % 500) >= 3: action = lkas_action else: action = 0xf diff --git a/selfdrive/car/ford/interface.py b/selfdrive/car/ford/interface.py index 7155b3ea82d887..0faaa3f6698481 100755 --- a/selfdrive/car/ford/interface.py +++ b/selfdrive/car/ford/interface.py @@ -21,7 +21,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None): ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.01], [0.005]] # TODO: tune this ret.lateralTuning.pid.kf = 1. / MAX_ANGLE # MAX Steer angle to normalize FF ret.steerActuatorDelay = 0.1 # Default delay, not measured yet - ret.steerLimitTimer = 0.8 + ret.steerLimitTimer = 1.0 ret.steerRateCost = 1.0 ret.centerToFront = ret.wheelbase * 0.44 tire_stiffness_factor = 0.5328 @@ -51,7 +51,7 @@ def update(self, c, can_strings): # events events = self.create_common_events(ret) - if self.CS.lkas_state not in [2, 3] and ret.vEgo > 13. * CV.MPH_TO_MS and ret.cruiseState.enabled: + if self.CS.lkas_state not in (2, 3) and ret.vEgo > 13. * CV.MPH_TO_MS and ret.cruiseState.enabled: events.add(car.CarEvent.EventName.steerTempUnavailable) ret.events = events.to_msg() diff --git a/selfdrive/car/ford/radar_interface.py b/selfdrive/car/ford/radar_interface.py index 20a435b0821dff..94eb8bb0cc71b1 100755 --- a/selfdrive/car/ford/radar_interface.py +++ b/selfdrive/car/ford/radar_interface.py @@ -7,12 +7,12 @@ RADAR_MSGS = list(range(0x500, 0x540)) + def _create_radar_can_parser(car_fingerprint): msg_n = len(RADAR_MSGS) signals = list(zip(['X_Rel'] * msg_n + ['Angle'] * msg_n + ['V_Rel'] * msg_n, - RADAR_MSGS * 3, - [0] * msg_n + [0] * msg_n + [0] * msg_n)) - checks = list(zip(RADAR_MSGS, [20]*msg_n)) + RADAR_MSGS * 3)) + checks = list(zip(RADAR_MSGS, [20] * msg_n)) return CANParser(DBC[car_fingerprint]['radar'], signals, checks, 1) diff --git a/selfdrive/car/ford/values.py b/selfdrive/car/ford/values.py index 7c6cc6d2dff938..e1e2206472f020 100644 --- a/selfdrive/car/ford/values.py +++ b/selfdrive/car/ford/values.py @@ -1,5 +1,3 @@ -# flake8: noqa - from selfdrive.car import dbc_dict from cereal import car Ecu = car.CarParams.Ecu diff --git a/selfdrive/car/fw_versions.py b/selfdrive/car/fw_versions.py index 27d697331eaa34..59fd4fe8a55050 100755 --- a/selfdrive/car/fw_versions.py +++ b/selfdrive/car/fw_versions.py @@ -253,11 +253,11 @@ def match_fw_to_car_exact(fw_versions_dict): addr = ecu[1:] found_version = fw_versions_dict.get(addr, None) ESSENTIAL_ECUS = [Ecu.engine, Ecu.eps, Ecu.esp, Ecu.fwdRadar, Ecu.fwdCamera, Ecu.vsa] - if ecu_type == Ecu.esp and candidate in [TOYOTA.RAV4, TOYOTA.COROLLA, TOYOTA.HIGHLANDER, TOYOTA.SIENNA, TOYOTA.LEXUS_IS] and found_version is None: + if ecu_type == Ecu.esp and candidate in (TOYOTA.RAV4, TOYOTA.COROLLA, TOYOTA.HIGHLANDER, TOYOTA.SIENNA, TOYOTA.LEXUS_IS) and found_version is None: continue # On some Toyota models, the engine can show on two different addresses - if ecu_type == Ecu.engine and candidate in [TOYOTA.CAMRY, TOYOTA.COROLLA_TSS2, TOYOTA.CHR, TOYOTA.LEXUS_IS] and found_version is None: + if ecu_type == Ecu.engine and candidate in (TOYOTA.CAMRY, TOYOTA.COROLLA_TSS2, TOYOTA.CHR, TOYOTA.LEXUS_IS) and found_version is None: continue # Ignore non essential ecus diff --git a/selfdrive/car/gm/carcontroller.py b/selfdrive/car/gm/carcontroller.py index 71587b17d96141..7ad1e7cf88885f 100644 --- a/selfdrive/car/gm/carcontroller.py +++ b/selfdrive/car/gm/carcontroller.py @@ -27,7 +27,7 @@ def __init__(self, dbc_name, CP, VM): self.packer_obj = CANPacker(DBC[CP.carFingerprint]['radar']) self.packer_ch = CANPacker(DBC[CP.carFingerprint]['chassis']) - def update(self, enabled, CS, frame, actuators, + def update(self, c, enabled, CS, frame, actuators, hud_v_cruise, hud_show_lanes, hud_show_car, hud_alert): P = self.params @@ -41,7 +41,7 @@ def update(self, enabled, CS, frame, actuators, if CS.lka_steering_cmd_counter != self.lka_steering_cmd_counter_last: self.lka_steering_cmd_counter_last = CS.lka_steering_cmd_counter elif (frame % P.STEER_STEP) == 0: - lkas_enabled = enabled and not (CS.out.steerWarning or CS.out.steerError) and CS.out.vEgo > P.MIN_STEER_SPEED + lkas_enabled = c.active and not (CS.out.steerWarning or CS.out.steerError) and CS.out.vEgo > P.MIN_STEER_SPEED if lkas_enabled: new_steer = int(round(actuators.steer * P.STEER_MAX)) apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, P) @@ -58,7 +58,7 @@ def update(self, enabled, CS, frame, actuators, # Gas/regen and brakes - all at 25Hz if (frame % 4) == 0: - if not enabled: + if not c.active: # Stock ECU sends max regen when not enabled. self.apply_gas = P.MAX_ACC_REGEN self.apply_brake = 0 @@ -105,7 +105,7 @@ def update(self, enabled, CS, frame, actuators, lka_critical = lka_active and abs(actuators.steer) > 0.9 lka_icon_status = (lka_active, lka_critical) if frame % P.CAMERA_KEEPALIVE_STEP == 0 or lka_icon_status != self.lka_icon_status_last: - steer_alert = hud_alert in [VisualAlert.steerRequired, VisualAlert.ldw] + steer_alert = hud_alert in (VisualAlert.steerRequired, VisualAlert.ldw) can_sends.append(gmcan.create_lka_icon_command(CanBus.SW_GMLAN, lka_active, lka_critical, steer_alert)) self.lka_icon_status_last = lka_icon_status diff --git a/selfdrive/car/gm/carstate.py b/selfdrive/car/gm/carstate.py index 5180d2e5110c60..134bd9d075ee9f 100644 --- a/selfdrive/car/gm/carstate.py +++ b/selfdrive/car/gm/carstate.py @@ -3,8 +3,7 @@ from opendbc.can.can_define import CANDefine from opendbc.can.parser import CANParser from selfdrive.car.interfaces import CarStateBase -from selfdrive.car.gm.values import DBC, CAR, AccState, CanBus, \ - CruiseButtons, STEER_THRESHOLD +from selfdrive.car.gm.values import DBC, CAR, AccState, CanBus, STEER_THRESHOLD class CarState(CarStateBase): @@ -79,33 +78,32 @@ def update(self, pt_cp, loopback_cp): @staticmethod def get_can_parser(CP): - # this function generates lists for signal, messages and initial values signals = [ - # sig_name, sig_address, default - ("BrakePedalPosition", "EBCMBrakePedalPosition", 0), - ("FrontLeftDoor", "BCMDoorBeltStatus", 0), - ("FrontRightDoor", "BCMDoorBeltStatus", 0), - ("RearLeftDoor", "BCMDoorBeltStatus", 0), - ("RearRightDoor", "BCMDoorBeltStatus", 0), - ("LeftSeatBelt", "BCMDoorBeltStatus", 0), - ("RightSeatBelt", "BCMDoorBeltStatus", 0), - ("TurnSignals", "BCMTurnSignals", 0), - ("AcceleratorPedal2", "AcceleratorPedal2", 0), - ("CruiseState", "AcceleratorPedal2", 0), - ("ACCButtons", "ASCMSteeringButton", CruiseButtons.UNPRESS), - ("SteeringWheelAngle", "PSCMSteeringAngle", 0), - ("SteeringWheelRate", "PSCMSteeringAngle", 0), - ("FLWheelSpd", "EBCMWheelSpdFront", 0), - ("FRWheelSpd", "EBCMWheelSpdFront", 0), - ("RLWheelSpd", "EBCMWheelSpdRear", 0), - ("RRWheelSpd", "EBCMWheelSpdRear", 0), - ("PRNDL", "ECMPRDNL", 0), - ("LKADriverAppldTrq", "PSCMStatus", 0), - ("LKATorqueDelivered", "PSCMStatus", 0), - ("LKATorqueDeliveredStatus", "PSCMStatus", 0), - ("TractionControlOn", "ESPStatus", 0), - ("EPBClosed", "EPBStatus", 0), - ("CruiseMainOn", "ECMEngineStatus", 0), + # sig_name, sig_address + ("BrakePedalPosition", "EBCMBrakePedalPosition"), + ("FrontLeftDoor", "BCMDoorBeltStatus"), + ("FrontRightDoor", "BCMDoorBeltStatus"), + ("RearLeftDoor", "BCMDoorBeltStatus"), + ("RearRightDoor", "BCMDoorBeltStatus"), + ("LeftSeatBelt", "BCMDoorBeltStatus"), + ("RightSeatBelt", "BCMDoorBeltStatus"), + ("TurnSignals", "BCMTurnSignals"), + ("AcceleratorPedal2", "AcceleratorPedal2"), + ("CruiseState", "AcceleratorPedal2"), + ("ACCButtons", "ASCMSteeringButton"), + ("SteeringWheelAngle", "PSCMSteeringAngle"), + ("SteeringWheelRate", "PSCMSteeringAngle"), + ("FLWheelSpd", "EBCMWheelSpdFront"), + ("FRWheelSpd", "EBCMWheelSpdFront"), + ("RLWheelSpd", "EBCMWheelSpdRear"), + ("RRWheelSpd", "EBCMWheelSpdRear"), + ("PRNDL", "ECMPRDNL"), + ("LKADriverAppldTrq", "PSCMStatus"), + ("LKATorqueDelivered", "PSCMStatus"), + ("LKATorqueDeliveredStatus", "PSCMStatus"), + ("TractionControlOn", "ESPStatus"), + ("EPBClosed", "EPBStatus"), + ("CruiseMainOn", "ECMEngineStatus"), ] checks = [ @@ -125,19 +123,15 @@ def get_can_parser(CP): ] if CP.carFingerprint == CAR.VOLT: - signals += [ - ("RegenPaddle", "EBCMRegenPaddle", 0), - ] - checks += [ - ("EBCMRegenPaddle", 50), - ] + signals.append(("RegenPaddle", "EBCMRegenPaddle")) + checks.append(("EBCMRegenPaddle", 50)) return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, CanBus.POWERTRAIN) @staticmethod def get_loopback_can_parser(CP): signals = [ - ("RollingCounter", "ASCMLKASteeringCmd", 0), + ("RollingCounter", "ASCMLKASteeringCmd"), ] checks = [ diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py index 7b86589de31d29..d6a2d3cfeed2b7 100755 --- a/selfdrive/car/gm/interface.py +++ b/selfdrive/car/gm/interface.py @@ -47,7 +47,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None): # These cars have been put into dashcam only due to both a lack of users and test coverage. # These cars likely still work fine. Once a user confirms each car works and a test route is # added to selfdrive/test/test_routes, we can remove it from this list. - ret.dashcamOnly = candidate in {CAR.CADILLAC_ATS, CAR.HOLDEN_ASTRA, CAR.MALIBU} + ret.dashcamOnly = candidate in {CAR.CADILLAC_ATS, CAR.HOLDEN_ASTRA, CAR.MALIBU, CAR.BUICK_REGAL} # Presence of a camera on the object bus is ok. # Have to go to read_only if ASCM is online (ACC-enabled cars), @@ -203,7 +203,7 @@ def update(self, c, can_strings): # handle button presses for b in ret.buttonEvents: # do enable on both accel and decel buttons - if b.type in [ButtonType.accelCruise, ButtonType.decelCruise] and not b.pressed: + if b.type in (ButtonType.accelCruise, ButtonType.decelCruise) and not b.pressed: events.add(EventName.buttonEnable) # do disable on button down if b.type == ButtonType.cancel and b.pressed: @@ -226,7 +226,7 @@ def apply(self, c): # In GM, PCM faults out if ACC command overlaps user gas. enabled = c.enabled and not self.CS.out.gasPressed - ret = self.CC.update(enabled, self.CS, self.frame, + ret = self.CC.update(c, enabled, self.CS, self.frame, c.actuators, hud_v_cruise, hud_control.lanesVisible, hud_control.leadVisible, hud_control.visualAlert) diff --git a/selfdrive/car/gm/radar_interface.py b/selfdrive/car/gm/radar_interface.py index d1ad1c16359247..66fac54748fb2c 100755 --- a/selfdrive/car/gm/radar_interface.py +++ b/selfdrive/car/gm/radar_interface.py @@ -14,6 +14,7 @@ # messages that are present in DBC LAST_RADAR_MSG = RADAR_HEADER_MSG + NUM_SLOTS + def create_radar_can_parser(car_fingerprint): if car_fingerprint not in (CAR.VOLT, CAR.MALIBU, CAR.HOLDEN_ASTRA, CAR.ACADIA, CAR.CADILLAC_ATS, CAR.ESCALADE_ESV): return None @@ -21,17 +22,13 @@ def create_radar_can_parser(car_fingerprint): # C1A-ARS3-A by Continental radar_targets = list(range(SLOT_1_MSG, SLOT_1_MSG + NUM_SLOTS)) signals = list(zip(['FLRRNumValidTargets', - 'FLRRSnsrBlckd', 'FLRRYawRtPlsblityFlt', - 'FLRRHWFltPrsntInt', 'FLRRAntTngFltPrsnt', - 'FLRRAlgnFltPrsnt', 'FLRRSnstvFltPrsntInt'] + - ['TrkRange'] * NUM_SLOTS + ['TrkRangeRate'] * NUM_SLOTS + - ['TrkRangeAccel'] * NUM_SLOTS + ['TrkAzimuth'] * NUM_SLOTS + - ['TrkWidth'] * NUM_SLOTS + ['TrkObjectID'] * NUM_SLOTS, - [RADAR_HEADER_MSG] * 7 + radar_targets * 6, - [0] * 7 + - [0.0] * NUM_SLOTS + [0.0] * NUM_SLOTS + - [0.0] * NUM_SLOTS + [0.0] * NUM_SLOTS + - [0.0] * NUM_SLOTS + [0] * NUM_SLOTS)) + 'FLRRSnsrBlckd', 'FLRRYawRtPlsblityFlt', + 'FLRRHWFltPrsntInt', 'FLRRAntTngFltPrsnt', + 'FLRRAlgnFltPrsnt', 'FLRRSnstvFltPrsntInt'] + + ['TrkRange'] * NUM_SLOTS + ['TrkRangeRate'] * NUM_SLOTS + + ['TrkRangeAccel'] * NUM_SLOTS + ['TrkAzimuth'] * NUM_SLOTS + + ['TrkWidth'] * NUM_SLOTS + ['TrkObjectID'] * NUM_SLOTS, + [RADAR_HEADER_MSG] * 7 + radar_targets * 6)) checks = list({(s[1], 14) for s in signals}) diff --git a/selfdrive/car/gm/values.py b/selfdrive/car/gm/values.py index d475b4bd671839..8e09076713e394 100644 --- a/selfdrive/car/gm/values.py +++ b/selfdrive/car/gm/values.py @@ -1,5 +1,3 @@ -# flake8: noqa - from cereal import car from selfdrive.car import dbc_dict Ecu = car.CarParams.Ecu @@ -115,11 +113,11 @@ class CanBus: } DBC = { - CAR.HOLDEN_ASTRA: dbc_dict('gm_global_a_powertrain', 'gm_global_a_object', chassis_dbc='gm_global_a_chassis'), - CAR.VOLT: dbc_dict('gm_global_a_powertrain', 'gm_global_a_object', chassis_dbc='gm_global_a_chassis'), - CAR.MALIBU: dbc_dict('gm_global_a_powertrain', 'gm_global_a_object', chassis_dbc='gm_global_a_chassis'), - CAR.ACADIA: dbc_dict('gm_global_a_powertrain', 'gm_global_a_object', chassis_dbc='gm_global_a_chassis'), - CAR.CADILLAC_ATS: dbc_dict('gm_global_a_powertrain', 'gm_global_a_object', chassis_dbc='gm_global_a_chassis'), - CAR.BUICK_REGAL: dbc_dict('gm_global_a_powertrain', 'gm_global_a_object', chassis_dbc='gm_global_a_chassis'), - CAR.ESCALADE_ESV: dbc_dict('gm_global_a_powertrain', 'gm_global_a_object', chassis_dbc='gm_global_a_chassis'), + CAR.HOLDEN_ASTRA: dbc_dict('gm_global_a_powertrain_generated', 'gm_global_a_object', chassis_dbc='gm_global_a_chassis'), + CAR.VOLT: dbc_dict('gm_global_a_powertrain_generated', 'gm_global_a_object', chassis_dbc='gm_global_a_chassis'), + CAR.MALIBU: dbc_dict('gm_global_a_powertrain_generated', 'gm_global_a_object', chassis_dbc='gm_global_a_chassis'), + CAR.ACADIA: dbc_dict('gm_global_a_powertrain_generated', 'gm_global_a_object', chassis_dbc='gm_global_a_chassis'), + CAR.CADILLAC_ATS: dbc_dict('gm_global_a_powertrain_generated', 'gm_global_a_object', chassis_dbc='gm_global_a_chassis'), + CAR.BUICK_REGAL: dbc_dict('gm_global_a_powertrain_generated', 'gm_global_a_object', chassis_dbc='gm_global_a_chassis'), + CAR.ESCALADE_ESV: dbc_dict('gm_global_a_powertrain_generated', 'gm_global_a_object', chassis_dbc='gm_global_a_chassis'), } diff --git a/selfdrive/car/honda/carcontroller.py b/selfdrive/car/honda/carcontroller.py index a09f418af5c41b..49581799a7d484 100644 --- a/selfdrive/car/honda/carcontroller.py +++ b/selfdrive/car/honda/carcontroller.py @@ -83,7 +83,7 @@ def process_hud_alert(hud_alert): # priority is: FCW, steer required, all others if hud_alert == VisualAlert.fcw: fcw_display = VISUAL_HUD[hud_alert.raw] - elif hud_alert in [VisualAlert.steerRequired, VisualAlert.ldw]: + elif hud_alert in (VisualAlert.steerRequired, VisualAlert.ldw): steer_required = VISUAL_HUD[hud_alert.raw] else: acc_alert = VISUAL_HUD[hud_alert.raw] @@ -117,7 +117,7 @@ def update(self, enabled, active, CS, frame, actuators, pcm_cancel_cmd, P = self.params - if enabled: + if active: accel = actuators.accel gas, brake = compute_gas_brake(actuators.accel, CS.out.vEgo, CS.CP.carFingerprint) else: @@ -152,7 +152,7 @@ def update(self, enabled, active, CS, frame, actuators, pcm_cancel_cmd, # steer torque is converted back to CAN reference (positive when steering right) apply_steer = int(interp(-actuators.steer * P.STEER_MAX, P.STEER_LOOKUP_BP, P.STEER_LOOKUP_V)) - lkas_active = enabled and not CS.steer_not_allowed + lkas_active = active and not CS.steer_not_allowed # Send CAN commands. can_sends = [] diff --git a/selfdrive/car/honda/carstate.py b/selfdrive/car/honda/carstate.py index 12a4ae4c29bdeb..d1907a9d223ba5 100644 --- a/selfdrive/car/honda/carstate.py +++ b/selfdrive/car/honda/carstate.py @@ -11,34 +11,33 @@ def get_can_signals(CP, gearbox_msg, main_on_sig_msg): - # this function generates lists for signal, messages and initial values signals = [ - ("XMISSION_SPEED", "ENGINE_DATA", 0), - ("WHEEL_SPEED_FL", "WHEEL_SPEEDS", 0), - ("WHEEL_SPEED_FR", "WHEEL_SPEEDS", 0), - ("WHEEL_SPEED_RL", "WHEEL_SPEEDS", 0), - ("WHEEL_SPEED_RR", "WHEEL_SPEEDS", 0), - ("STEER_ANGLE", "STEERING_SENSORS", 0), - ("STEER_ANGLE_RATE", "STEERING_SENSORS", 0), - ("MOTOR_TORQUE", "STEER_MOTOR_TORQUE", 0), - ("STEER_TORQUE_SENSOR", "STEER_STATUS", 0), - ("LEFT_BLINKER", "SCM_FEEDBACK", 0), - ("RIGHT_BLINKER", "SCM_FEEDBACK", 0), - ("GEAR", gearbox_msg, 0), - ("SEATBELT_DRIVER_LAMP", "SEATBELT_STATUS", 1), - ("SEATBELT_DRIVER_LATCHED", "SEATBELT_STATUS", 0), - ("BRAKE_PRESSED", "POWERTRAIN_DATA", 0), - ("BRAKE_SWITCH", "POWERTRAIN_DATA", 0), - ("CRUISE_BUTTONS", "SCM_BUTTONS", 0), - ("ESP_DISABLED", "VSA_STATUS", 1), - ("USER_BRAKE", "VSA_STATUS", 0), - ("BRAKE_HOLD_ACTIVE", "VSA_STATUS", 0), - ("STEER_STATUS", "STEER_STATUS", 5), - ("GEAR_SHIFTER", gearbox_msg, 0), - ("PEDAL_GAS", "POWERTRAIN_DATA", 0), - ("CRUISE_SETTING", "SCM_BUTTONS", 0), - ("ACC_STATUS", "POWERTRAIN_DATA", 0), - ("MAIN_ON", main_on_sig_msg, 0), + ("XMISSION_SPEED", "ENGINE_DATA"), + ("WHEEL_SPEED_FL", "WHEEL_SPEEDS"), + ("WHEEL_SPEED_FR", "WHEEL_SPEEDS"), + ("WHEEL_SPEED_RL", "WHEEL_SPEEDS"), + ("WHEEL_SPEED_RR", "WHEEL_SPEEDS"), + ("STEER_ANGLE", "STEERING_SENSORS"), + ("STEER_ANGLE_RATE", "STEERING_SENSORS"), + ("MOTOR_TORQUE", "STEER_MOTOR_TORQUE"), + ("STEER_TORQUE_SENSOR", "STEER_STATUS"), + ("LEFT_BLINKER", "SCM_FEEDBACK"), + ("RIGHT_BLINKER", "SCM_FEEDBACK"), + ("GEAR", gearbox_msg), + ("SEATBELT_DRIVER_LAMP", "SEATBELT_STATUS"), + ("SEATBELT_DRIVER_LATCHED", "SEATBELT_STATUS"), + ("BRAKE_PRESSED", "POWERTRAIN_DATA"), + ("BRAKE_SWITCH", "POWERTRAIN_DATA"), + ("CRUISE_BUTTONS", "SCM_BUTTONS"), + ("ESP_DISABLED", "VSA_STATUS"), + ("USER_BRAKE", "VSA_STATUS"), + ("BRAKE_HOLD_ACTIVE", "VSA_STATUS"), + ("STEER_STATUS", "STEER_STATUS"), + ("GEAR_SHIFTER", gearbox_msg), + ("PEDAL_GAS", "POWERTRAIN_DATA"), + ("CRUISE_SETTING", "SCM_BUTTONS"), + ("ACC_STATUS", "POWERTRAIN_DATA"), + ("MAIN_ON", main_on_sig_msg), ] checks = [ @@ -65,22 +64,18 @@ def get_can_signals(CP, gearbox_msg, main_on_sig_msg): ] if CP.carFingerprint in (CAR.CRV_HYBRID, CAR.CIVIC_BOSCH_DIESEL, CAR.ACURA_RDX_3G, CAR.HONDA_E): - checks += [ - (gearbox_msg, 50), - ] + checks.append((gearbox_msg, 50)) else: - checks += [ - (gearbox_msg, 100), - ] + checks.append((gearbox_msg, 100)) if CP.carFingerprint in HONDA_BOSCH_ALT_BRAKE_SIGNAL: - signals += [("BRAKE_PRESSED", "BRAKE_MODULE", 0)] - checks += [("BRAKE_MODULE", 50)] + signals.append(("BRAKE_PRESSED", "BRAKE_MODULE")) + checks.append(("BRAKE_MODULE", 50)) if CP.carFingerprint in HONDA_BOSCH: signals += [ - ("EPB_STATE", "EPB_STATUS", 0), - ("IMPERIAL_UNIT", "CAR_SPEED", 1), + ("EPB_STATE", "EPB_STATUS"), + ("IMPERIAL_UNIT", "CAR_SPEED"), ] checks += [ ("EPB_STATUS", 50), @@ -89,65 +84,65 @@ def get_can_signals(CP, gearbox_msg, main_on_sig_msg): if not CP.openpilotLongitudinalControl: signals += [ - ("CRUISE_CONTROL_LABEL", "ACC_HUD", 0), - ("CRUISE_SPEED", "ACC_HUD", 0), - ("ACCEL_COMMAND", "ACC_CONTROL", 0), - ("AEB_STATUS", "ACC_CONTROL", 0), + ("CRUISE_CONTROL_LABEL", "ACC_HUD"), + ("CRUISE_SPEED", "ACC_HUD"), + ("ACCEL_COMMAND", "ACC_CONTROL"), + ("AEB_STATUS", "ACC_CONTROL"), ] checks += [ ("ACC_HUD", 10), ("ACC_CONTROL", 50), ] else: # Nidec signals - signals += [("CRUISE_SPEED_PCM", "CRUISE", 0), - ("CRUISE_SPEED_OFFSET", "CRUISE_PARAMS", 0)] + signals += [("CRUISE_SPEED_PCM", "CRUISE"), + ("CRUISE_SPEED_OFFSET", "CRUISE_PARAMS")] if CP.carFingerprint == CAR.ODYSSEY_CHN: - checks += [("CRUISE_PARAMS", 10)] + checks.append(("CRUISE_PARAMS", 10)) else: - checks += [("CRUISE_PARAMS", 50)] + checks.append(("CRUISE_PARAMS", 50)) if CP.carFingerprint in (CAR.ACCORD, CAR.ACCORDH, CAR.CIVIC_BOSCH, CAR.CIVIC_BOSCH_DIESEL, CAR.CRV_HYBRID, CAR.INSIGHT, CAR.ACURA_RDX_3G, CAR.HONDA_E): - signals += [("DRIVERS_DOOR_OPEN", "SCM_FEEDBACK", 1)] + signals.append(("DRIVERS_DOOR_OPEN", "SCM_FEEDBACK")) elif CP.carFingerprint == CAR.ODYSSEY_CHN: - signals += [("DRIVERS_DOOR_OPEN", "SCM_BUTTONS", 1)] + signals.append(("DRIVERS_DOOR_OPEN", "SCM_BUTTONS")) elif CP.carFingerprint in (CAR.FREED, CAR.HRV): - signals += [("DRIVERS_DOOR_OPEN", "SCM_BUTTONS", 1), - ("WHEELS_MOVING", "STANDSTILL", 1)] + signals += [("DRIVERS_DOOR_OPEN", "SCM_BUTTONS"), + ("WHEELS_MOVING", "STANDSTILL")] else: - signals += [("DOOR_OPEN_FL", "DOORS_STATUS", 1), - ("DOOR_OPEN_FR", "DOORS_STATUS", 1), - ("DOOR_OPEN_RL", "DOORS_STATUS", 1), - ("DOOR_OPEN_RR", "DOORS_STATUS", 1), - ("WHEELS_MOVING", "STANDSTILL", 1)] + signals += [("DOOR_OPEN_FL", "DOORS_STATUS"), + ("DOOR_OPEN_FR", "DOORS_STATUS"), + ("DOOR_OPEN_RL", "DOORS_STATUS"), + ("DOOR_OPEN_RR", "DOORS_STATUS"), + ("WHEELS_MOVING", "STANDSTILL")] checks += [ ("DOORS_STATUS", 3), ("STANDSTILL", 50), ] if CP.carFingerprint == CAR.CIVIC: - signals += [("IMPERIAL_UNIT", "HUD_SETTING", 0), - ("EPB_STATE", "EPB_STATUS", 0)] + signals += [("IMPERIAL_UNIT", "HUD_SETTING"), + ("EPB_STATE", "EPB_STATUS")] checks += [ ("HUD_SETTING", 50), ("EPB_STATUS", 50), ] elif CP.carFingerprint in (CAR.ODYSSEY, CAR.ODYSSEY_CHN): - signals += [("EPB_STATE", "EPB_STATUS", 0)] - checks += [("EPB_STATUS", 50)] + signals.append(("EPB_STATE", "EPB_STATUS")) + checks.append(("EPB_STATUS", 50)) # add gas interceptor reading if we are using it if CP.enableGasInterceptor: - signals.append(("INTERCEPTOR_GAS", "GAS_SENSOR", 0)) - signals.append(("INTERCEPTOR_GAS2", "GAS_SENSOR", 0)) + signals.append(("INTERCEPTOR_GAS", "GAS_SENSOR")) + signals.append(("INTERCEPTOR_GAS2", "GAS_SENSOR")) checks.append(("GAS_SENSOR", 50)) if CP.openpilotLongitudinalControl: signals += [ - ("BRAKE_ERROR_1", "STANDSTILL", 1), - ("BRAKE_ERROR_2", "STANDSTILL", 1) + ("BRAKE_ERROR_1", "STANDSTILL"), + ("BRAKE_ERROR_2", "STANDSTILL") ] - checks += [("STANDSTILL", 50)] + checks.append(("STANDSTILL", 50)) return signals, checks @@ -167,6 +162,7 @@ def __init__(self, CP): self.shifter_values = can_define.dv[self.gearbox_msg]["GEAR_SHIFTER"] self.steer_status_values = defaultdict(lambda: "UNKNOWN", can_define.dv["STEER_STATUS"]["STEER_STATUS"]) + self.brake_error = False self.brake_switch_prev = 0 self.brake_switch_prev_ts = 0 self.cruise_setting = 0 @@ -201,15 +197,13 @@ def update(self, cp, cp_cam, cp_body): ret.seatbeltUnlatched = bool(cp.vl["SEATBELT_STATUS"]["SEATBELT_DRIVER_LAMP"] or not cp.vl["SEATBELT_STATUS"]["SEATBELT_DRIVER_LATCHED"]) steer_status = self.steer_status_values[cp.vl["STEER_STATUS"]["STEER_STATUS"]] - ret.steerError = steer_status not in ["NORMAL", "NO_TORQUE_ALERT_1", "NO_TORQUE_ALERT_2", "LOW_SPEED_LOCKOUT", "TMP_FAULT"] + ret.steerError = steer_status not in ("NORMAL", "NO_TORQUE_ALERT_1", "NO_TORQUE_ALERT_2", "LOW_SPEED_LOCKOUT", "TMP_FAULT") # NO_TORQUE_ALERT_2 can be caused by bump OR steering nudge from driver - self.steer_not_allowed = steer_status not in ["NORMAL", "NO_TORQUE_ALERT_2"] + self.steer_not_allowed = steer_status not in ("NORMAL", "NO_TORQUE_ALERT_2") # LOW_SPEED_LOCKOUT is not worth a warning - ret.steerWarning = steer_status not in ["NORMAL", "LOW_SPEED_LOCKOUT", "NO_TORQUE_ALERT_2"] + ret.steerWarning = steer_status not in ("NORMAL", "LOW_SPEED_LOCKOUT", "NO_TORQUE_ALERT_2") - if not self.CP.openpilotLongitudinalControl: - self.brake_error = 0 - else: + if self.CP.openpilotLongitudinalControl: self.brake_error = cp.vl["STANDSTILL"]["BRAKE_ERROR_1"] or cp.vl["STANDSTILL"]["BRAKE_ERROR_2"] ret.espDisabled = cp.vl["VSA_STATUS"]["ESP_DISABLED"] != 0 @@ -245,15 +239,11 @@ def update(self, cp, cp_cam, cp_body): gear = int(cp.vl[self.gearbox_msg]["GEAR_SHIFTER"]) ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(gear, None)) - ret.gas = cp.vl["POWERTRAIN_DATA"]["PEDAL_GAS"] - - # this is a hack for the interceptor. This is now only used in the simulation - # TODO: Replace tests by toyota so this can go away if self.CP.enableGasInterceptor: - user_gas = (cp.vl["GAS_SENSOR"]["INTERCEPTOR_GAS"] + cp.vl["GAS_SENSOR"]["INTERCEPTOR_GAS2"]) / 2. - ret.gasPressed = user_gas > 1e-5 # this works because interceptor reads < 0 when pedal position is 0. Once calibrated, this will change + ret.gas = (cp.vl["GAS_SENSOR"]["INTERCEPTOR_GAS"] + cp.vl["GAS_SENSOR"]["INTERCEPTOR_GAS2"]) / 2. else: - ret.gasPressed = ret.gas > 1e-5 + ret.gas = cp.vl["POWERTRAIN_DATA"]["PEDAL_GAS"] + ret.gasPressed = ret.gas > 1e-5 ret.steeringTorque = cp.vl["STEER_STATUS"]["STEER_TORQUE_SENSOR"] ret.steeringTorqueEps = cp.vl["STEER_MOTOR_TORQUE"]["MOTOR_TORQUE"] @@ -288,8 +278,8 @@ def update(self, cp, cp_cam, cp_body): ret.cruiseState.available = bool(cp.vl[self.main_on_sig_msg]["MAIN_ON"]) # Gets rid of Pedal Grinding noise when brake is pressed at slow speeds for some models - if self.CP.carFingerprint in (CAR.PILOT, CAR.PILOT_2019, CAR.PASSPORT, CAR.RIDGELINE): - if ret.brake > 0.05: + if self.CP.carFingerprint in (CAR.PILOT, CAR.PASSPORT, CAR.RIDGELINE): + if ret.brake > 0.1: ret.brakePressed = True # TODO: discover the CAN msg that has the imperial unit bit for all other cars @@ -334,14 +324,14 @@ def get_cam_can_parser(CP): ] if CP.carFingerprint not in HONDA_BOSCH: - signals += [("COMPUTER_BRAKE", "BRAKE_COMMAND", 0), - ("AEB_REQ_1", "BRAKE_COMMAND", 0), - ("FCW", "BRAKE_COMMAND", 0), - ("CHIME", "BRAKE_COMMAND", 0), - ("FCM_OFF", "ACC_HUD", 0), - ("FCM_OFF_2", "ACC_HUD", 0), - ("FCM_PROBLEM", "ACC_HUD", 0), - ("ICONS", "ACC_HUD", 0)] + signals += [("COMPUTER_BRAKE", "BRAKE_COMMAND"), + ("AEB_REQ_1", "BRAKE_COMMAND"), + ("FCW", "BRAKE_COMMAND"), + ("CHIME", "BRAKE_COMMAND"), + ("FCM_OFF", "ACC_HUD"), + ("FCM_OFF_2", "ACC_HUD"), + ("FCM_PROBLEM", "ACC_HUD"), + ("ICONS", "ACC_HUD")] checks += [ ("ACC_HUD", 10), ("BRAKE_COMMAND", 50), @@ -352,8 +342,8 @@ def get_cam_can_parser(CP): @staticmethod def get_body_can_parser(CP): if CP.enableBsm and CP.carFingerprint == CAR.CRV_5G: - signals = [("BSM_ALERT", "BSM_STATUS_RIGHT", 0), - ("BSM_ALERT", "BSM_STATUS_LEFT", 0)] + signals = [("BSM_ALERT", "BSM_STATUS_RIGHT"), + ("BSM_ALERT", "BSM_STATUS_LEFT")] checks = [ ("BSM_STATUS_LEFT", 3), diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py index a1828622c4f03b..94e43059093ff9 100755 --- a/selfdrive/car/honda/interface.py +++ b/selfdrive/car/honda/interface.py @@ -47,7 +47,6 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[]): # py ret.openpilotLongitudinalControl = True ret.pcmCruise = not ret.enableGasInterceptor - ret.communityFeature = ret.enableGasInterceptor if candidate == CAR.CRV_5G: ret.enableBsm = 0x12f8bfa7 in fingerprint[0] @@ -250,17 +249,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[]): # py tire_stiffness_factor = 0.82 ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.28], [0.08]] - elif candidate in (CAR.PILOT, CAR.PILOT_2019): - stop_and_go = False - ret.mass = 4204. * CV.LB_TO_KG + STD_CARGO_KG # average weight - ret.wheelbase = 2.82 - ret.centerToFront = ret.wheelbase * 0.428 - ret.steerRatio = 17.25 # as spec - ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end - tire_stiffness_factor = 0.444 - ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.38], [0.11]] - - elif candidate == CAR.PASSPORT: + elif candidate in (CAR.PILOT, CAR.PASSPORT): stop_and_go = False ret.mass = 4204. * CV.LB_TO_KG + STD_CARGO_KG # average weight ret.wheelbase = 2.82 @@ -416,7 +405,7 @@ def update(self, c, can_strings): for b in ret.buttonEvents: # do enable on both accel and decel buttons - if b.type in [ButtonType.accelCruise, ButtonType.decelCruise] and not b.pressed: + if b.type in (ButtonType.accelCruise, ButtonType.decelCruise) and not b.pressed: if not self.CP.pcmCruise: events.add(EventName.buttonEnable) diff --git a/selfdrive/car/honda/radar_interface.py b/selfdrive/car/honda/radar_interface.py index 45e015af6e7f73..629ab01d4cfe92 100755 --- a/selfdrive/car/honda/radar_interface.py +++ b/selfdrive/car/honda/radar_interface.py @@ -4,13 +4,13 @@ from selfdrive.car.interfaces import RadarInterfaceBase from selfdrive.car.honda.values import DBC + def _create_nidec_can_parser(car_fingerprint): radar_messages = [0x400] + list(range(0x430, 0x43A)) + list(range(0x440, 0x446)) signals = list(zip(['RADAR_STATE'] + - ['LONG_DIST'] * 16 + ['NEW_TRACK'] * 16 + ['LAT_DIST'] * 16 + - ['REL_SPEED'] * 16, - [0x400] + radar_messages[1:] * 4, - [0] + [255] * 16 + [1] * 16 + [0] * 16 + [0] * 16)) + ['LONG_DIST'] * 16 + ['NEW_TRACK'] * 16 + ['LAT_DIST'] * 16 + + ['REL_SPEED'] * 16, + [0x400] + radar_messages[1:] * 4)) checks = [(s[1], 20) for s in signals] return CANParser(DBC[car_fingerprint]['radar'], signals, checks, 1) diff --git a/selfdrive/car/honda/values.py b/selfdrive/car/honda/values.py index cbe2369b1457c4..6e347b8da30d85 100644 --- a/selfdrive/car/honda/values.py +++ b/selfdrive/car/honda/values.py @@ -82,7 +82,6 @@ class CAR: ACURA_RDX = "ACURA RDX 2018" ACURA_RDX_3G = "ACURA RDX 2020" PILOT = "HONDA PILOT 2017" - PILOT_2019 = "HONDA PILOT 2019" PASSPORT = "HONDA PASSPORT 2021" RIDGELINE = "HONDA RIDGELINE 2017" INSIGHT = "HONDA INSIGHT 2019" @@ -990,74 +989,51 @@ class CAR: b'37805-RLV-C530\x00\x00', b'37805-RLV-C910\x00\x00', ], + (Ecu.gateway, 0x18daeff1, None): [ + b'38897-TG7-A030\x00\x00', + b'38897-TG7-A040\x00\x00', + b'38897-TG7-A110\x00\x00', + b'38897-TG7-A210\x00\x00', + ], (Ecu.eps, 0x18da30f1, None): [ b'39990-TG7-A030\x00\x00', b'39990-TG7-A040\x00\x00', b'39990-TG7-A060\x00\x00', - ], - (Ecu.fwdCamera, 0x18dab0f1, None): [ - b'36161-TG7-A520\x00\x00', - b'36161-TG7-A720\x00\x00', - b'36161-TG7-A820\x00\x00', - b'36161-TG7-C520\x00\x00', - b'36161-TG7-D520\x00\x00', - b'36161-TG8-A520\x00\x00', - b'36161-TG8-A720\x00\x00', - ], - (Ecu.srs, 0x18da53f1, None): [ - b'77959-TG7-A110\x00\x00', - b'77959-TG7-A020\x00\x00', - ], - (Ecu.combinationMeter, 0x18da60f1, None): [ - b'78109-TG7-A040\x00\x00', - b'78109-TG7-A050\x00\x00', - b'78109-TG7-A420\x00\x00', - b'78109-TG7-A520\x00\x00', - b'78109-TG7-A720\x00\x00', - b'78109-TG7-D020\x00\x00', - b'78109-TG8-A420\x00\x00', - b'78109-TG8-A520\x00\x00', - ], - (Ecu.vsa, 0x18da28f1, None): [ - b'57114-TG7-A130\x00\x00', - b'57114-TG7-A140\x00\x00', - b'57114-TG7-A230\x00\x00', - b'57114-TG7-A240\x00\x00', - b'57114-TG8-A140\x00\x00', - b'57114-TG8-A240\x00\x00', - ], - - }, - CAR.PILOT_2019: { - (Ecu.eps, 0x18da30f1, None): [ - b'39990-TG7-A060\x00\x00', b'39990-TG7-A070\x00\x00', b'39990-TGS-A230\x00\x00', ], - (Ecu.gateway, 0x18daeff1, None): [ - b'38897-TG7-A030\x00\x00', - b'38897-TG7-A040\x00\x00', - b'38897-TG7-A110\x00\x00', - b'38897-TG7-A210\x00\x00', - ], (Ecu.fwdCamera, 0x18dab0f1, None): [ b'36161-TG7-A310\x00\x00', + b'36161-TG7-A520\x00\x00', b'36161-TG7-A630\x00\x00', + b'36161-TG7-A720\x00\x00', + b'36161-TG7-A820\x00\x00', b'36161-TG7-A930\x00\x00', + b'36161-TG7-C520\x00\x00', + b'36161-TG7-D520\x00\x00', b'36161-TG7-D630\x00\x00', b'36161-TG7-Y630\x00\x00', + b'36161-TG8-A520\x00\x00', b'36161-TG8-A630\x00\x00', + b'36161-TG8-A720\x00\x00', b'36161-TG8-A830\x00\x00', b'36161-TGS-A130\x00\x00', b'36161-TGT-A030\x00\x00', b'36161-TGT-A130\x00\x00', ], (Ecu.srs, 0x18da53f1, None): [ + b'77959-TG7-A020\x00\x00', + b'77959-TG7-A110\x00\x00', b'77959-TG7-A210\x00\x00', b'77959-TG7-Y210\x00\x00', b'77959-TGS-A010\x00\x00', ], (Ecu.combinationMeter, 0x18da60f1, None): [ + b'78109-TG7-A040\x00\x00', + b'78109-TG7-A050\x00\x00', + b'78109-TG7-A420\x00\x00', + b'78109-TG7-A520\x00\x00', + b'78109-TG7-A720\x00\x00', b'78109-TG7-AJ10\x00\x00', b'78109-TG7-AJ20\x00\x00', b'78109-TG7-AK10\x00\x00', @@ -1069,8 +1045,11 @@ class CAR: b'78109-TG7-AT20\x00\x00', b'78109-TG7-AU20\x00\x00', b'78109-TG7-AX20\x00\x00', + b'78109-TG7-D020\x00\x00', b'78109-TG7-DJ10\x00\x00', b'78109-TG7-YK20\x00\x00', + b'78109-TG8-A420\x00\x00', + b'78109-TG8-A520\x00\x00', b'78109-TG8-AJ10\x00\x00', b'78109-TG8-AJ20\x00\x00', b'78109-TG8-AK20\x00\x00', @@ -1080,8 +1059,14 @@ class CAR: b'78109-TGT-AK30\x00\x00', ], (Ecu.vsa, 0x18da28f1, None): [ + b'57114-TG7-A130\x00\x00', + b'57114-TG7-A140\x00\x00', + b'57114-TG7-A230\x00\x00', + b'57114-TG7-A240\x00\x00', b'57114-TG7-A630\x00\x00', b'57114-TG7-A730\x00\x00', + b'57114-TG8-A140\x00\x00', + b'57114-TG8-A240\x00\x00', b'57114-TG8-A630\x00\x00', b'57114-TG8-A730\x00\x00', b'57114-TGS-A530\x00\x00', @@ -1371,10 +1356,9 @@ class CAR: CAR.HRV: dbc_dict('honda_fit_ex_2018_can_generated', 'acura_ilx_2016_nidec'), CAR.ODYSSEY: dbc_dict('honda_odyssey_exl_2018_generated', 'acura_ilx_2016_nidec'), CAR.ODYSSEY_CHN: dbc_dict('honda_odyssey_extreme_edition_2018_china_can_generated', 'acura_ilx_2016_nidec'), - CAR.PILOT: dbc_dict('honda_pilot_touring_2017_can_generated', 'acura_ilx_2016_nidec'), - CAR.PILOT_2019: dbc_dict('honda_pilot_touring_2017_can_generated', 'acura_ilx_2016_nidec'), - CAR.PASSPORT: dbc_dict('honda_pilot_touring_2017_can_generated', 'acura_ilx_2016_nidec'), - CAR.RIDGELINE: dbc_dict('honda_ridgeline_black_edition_2017_can_generated', 'acura_ilx_2016_nidec'), + CAR.PILOT: dbc_dict('acura_ilx_2016_can_generated', 'acura_ilx_2016_nidec'), + CAR.PASSPORT: dbc_dict('acura_ilx_2016_can_generated', 'acura_ilx_2016_nidec'), + CAR.RIDGELINE: dbc_dict('acura_ilx_2016_can_generated', 'acura_ilx_2016_nidec'), CAR.INSIGHT: dbc_dict('honda_insight_ex_2019_can_generated', None), CAR.HONDA_E: dbc_dict('acura_rdx_2020_can_generated', None), } @@ -1387,7 +1371,7 @@ class CAR: HONDA_NIDEC_ALT_PCM_ACCEL = {CAR.ODYSSEY} HONDA_NIDEC_ALT_SCM_MESSAGES = {CAR.ACURA_ILX, CAR.ACURA_RDX, CAR.CRV, CAR.CRV_EU, CAR.FIT, CAR.FREED, CAR.HRV, CAR.ODYSSEY_CHN, - CAR.PILOT, CAR.PILOT_2019, CAR.PASSPORT, CAR.RIDGELINE} + CAR.PILOT, CAR.PASSPORT, CAR.RIDGELINE} HONDA_BOSCH = {CAR.ACCORD, CAR.ACCORDH, CAR.CIVIC_BOSCH, CAR.CIVIC_BOSCH_DIESEL, CAR.CRV_5G, - CAR.CRV_HYBRID, CAR.INSIGHT, CAR.ACURA_RDX_3G, CAR.HONDA_E} + CAR.CRV_HYBRID, CAR.INSIGHT, CAR.ACURA_RDX_3G, CAR.HONDA_E} HONDA_BOSCH_ALT_BRAKE_SIGNAL = {CAR.ACCORD, CAR.CRV_5G, CAR.ACURA_RDX_3G} diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py index 61fb35664bdf41..f7c43bd6e32c02 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -13,7 +13,7 @@ def process_hud_alert(enabled, fingerprint, visual_alert, left_lane, right_lane, left_lane_depart, right_lane_depart): - sys_warning = (visual_alert in [VisualAlert.steerRequired, VisualAlert.ldw]) + sys_warning = (visual_alert in (VisualAlert.steerRequired, VisualAlert.ldw)) # initialize to no line visible sys_state = 1 @@ -28,9 +28,9 @@ def process_hud_alert(enabled, fingerprint, visual_alert, left_lane, left_lane_warning = 0 right_lane_warning = 0 if left_lane_depart: - left_lane_warning = 1 if fingerprint in [CAR.GENESIS_G90, CAR.GENESIS_G80] else 2 + left_lane_warning = 1 if fingerprint in (CAR.GENESIS_G90, CAR.GENESIS_G80) else 2 if right_lane_depart: - right_lane_warning = 1 if fingerprint in [CAR.GENESIS_G90, CAR.GENESIS_G80] else 2 + right_lane_warning = 1 if fingerprint in (CAR.GENESIS_G90, CAR.GENESIS_G80) else 2 return sys_warning, sys_state, left_lane_warning, right_lane_warning @@ -46,7 +46,7 @@ def __init__(self, dbc_name, CP, VM): self.last_resume_frame = 0 self.accel = 0 - def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, visual_alert, hud_speed, + def update(self, c, enabled, CS, frame, actuators, pcm_cancel_cmd, visual_alert, hud_speed, left_lane, right_lane, left_lane_depart, right_lane_depart): # Steering Torque new_steer = int(round(actuators.steer * self.p.STEER_MAX)) @@ -54,7 +54,7 @@ def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, visual_alert, hu self.steer_rate_limited = new_steer != apply_steer # disable when temp fault is active, or below LKA minimum speed - lkas_active = enabled and not CS.out.steerWarning and CS.out.vEgo >= CS.CP.minSteerSpeed + lkas_active = c.active and not CS.out.steerWarning and CS.out.vEgo >= CS.CP.minSteerSpeed if not lkas_active: apply_steer = 0 @@ -89,7 +89,7 @@ def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, visual_alert, hu if frame % 2 == 0 and CS.CP.openpilotLongitudinalControl: lead_visible = False - accel = actuators.accel if enabled else 0 + accel = actuators.accel if c.active else 0 jerk = clip(2.0 * (accel - CS.out.aEgo), -12.7, 12.7) @@ -104,10 +104,10 @@ def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, visual_alert, hu self.accel = accel # 20 Hz LFA MFA message - if frame % 5 == 0 and self.car_fingerprint in [CAR.SONATA, CAR.PALISADE, CAR.IONIQ, CAR.KIA_NIRO_EV, CAR.KIA_NIRO_HEV_2021, + if frame % 5 == 0 and self.car_fingerprint in (CAR.SONATA, CAR.PALISADE, CAR.IONIQ, CAR.KIA_NIRO_EV, CAR.KIA_NIRO_HEV_2021, CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV, CAR.KIA_CEED, CAR.KIA_SELTOS, CAR.KONA_EV, CAR.ELANTRA_2021, CAR.ELANTRA_HEV_2021, CAR.SONATA_HYBRID, CAR.KONA_HEV, CAR.SANTA_FE_2022, - CAR.KIA_K5_2021, CAR.IONIQ_HEV_2022, CAR.SANTA_FE_HEV_2022, CAR.GENESIS_G70_2020, CAR.SANTA_FE_PHEV_2022]: + CAR.KIA_K5_2021, CAR.IONIQ_HEV_2022, CAR.SANTA_FE_HEV_2022, CAR.GENESIS_G70_2020, CAR.SANTA_FE_PHEV_2022): can_sends.append(create_lfahda_mfc(self.packer, enabled)) # 5 Hz ACC options diff --git a/selfdrive/car/hyundai/carstate.py b/selfdrive/car/hyundai/carstate.py index e889f24fc944bf..bdd49e2067630d 100644 --- a/selfdrive/car/hyundai/carstate.py +++ b/selfdrive/car/hyundai/carstate.py @@ -120,57 +120,57 @@ def update(self, cp, cp_cam): @staticmethod def get_can_parser(CP): signals = [ - # sig_name, sig_address, default - ("WHL_SPD_FL", "WHL_SPD11", 0), - ("WHL_SPD_FR", "WHL_SPD11", 0), - ("WHL_SPD_RL", "WHL_SPD11", 0), - ("WHL_SPD_RR", "WHL_SPD11", 0), - - ("YAW_RATE", "ESP12", 0), - - ("CF_Gway_DrvSeatBeltInd", "CGW4", 1), - - ("CF_Gway_DrvSeatBeltSw", "CGW1", 0), - ("CF_Gway_DrvDrSw", "CGW1", 0), # Driver Door - ("CF_Gway_AstDrSw", "CGW1", 0), # Passenger door - ("CF_Gway_RLDrSw", "CGW2", 0), # Rear reft door - ("CF_Gway_RRDrSw", "CGW2", 0), # Rear right door - ("CF_Gway_TurnSigLh", "CGW1", 0), - ("CF_Gway_TurnSigRh", "CGW1", 0), - ("CF_Gway_ParkBrakeSw", "CGW1", 0), - - ("CYL_PRES", "ESP12", 0), - - ("CF_Clu_CruiseSwState", "CLU11", 0), - ("CF_Clu_CruiseSwMain", "CLU11", 0), - ("CF_Clu_SldMainSW", "CLU11", 0), - ("CF_Clu_ParityBit1", "CLU11", 0), - ("CF_Clu_VanzDecimal" , "CLU11", 0), - ("CF_Clu_Vanz", "CLU11", 0), - ("CF_Clu_SPEED_UNIT", "CLU11", 0), - ("CF_Clu_DetentOut", "CLU11", 0), - ("CF_Clu_RheostatLevel", "CLU11", 0), - ("CF_Clu_CluInfo", "CLU11", 0), - ("CF_Clu_AmpInfo", "CLU11", 0), - ("CF_Clu_AliveCnt1", "CLU11", 0), - - ("ACCEnable", "TCS13", 0), - ("ACC_REQ", "TCS13", 0), - ("DriverBraking", "TCS13", 0), - ("StandStill", "TCS13", 0), - ("PBRAKE_ACT", "TCS13", 0), - - ("ESC_Off_Step", "TCS15", 0), - ("AVH_LAMP", "TCS15", 0), - - ("CR_Mdps_StrColTq", "MDPS12", 0), - ("CF_Mdps_ToiActive", "MDPS12", 0), - ("CF_Mdps_ToiUnavail", "MDPS12", 0), - ("CF_Mdps_ToiFlt", "MDPS12", 0), - ("CR_Mdps_OutTq", "MDPS12", 0), - - ("SAS_Angle", "SAS11", 0), - ("SAS_Speed", "SAS11", 0), + # sig_name, sig_address + ("WHL_SPD_FL", "WHL_SPD11"), + ("WHL_SPD_FR", "WHL_SPD11"), + ("WHL_SPD_RL", "WHL_SPD11"), + ("WHL_SPD_RR", "WHL_SPD11"), + + ("YAW_RATE", "ESP12"), + + ("CF_Gway_DrvSeatBeltInd", "CGW4"), + + ("CF_Gway_DrvSeatBeltSw", "CGW1"), + ("CF_Gway_DrvDrSw", "CGW1"), # Driver Door + ("CF_Gway_AstDrSw", "CGW1"), # Passenger door + ("CF_Gway_RLDrSw", "CGW2"), # Rear reft door + ("CF_Gway_RRDrSw", "CGW2"), # Rear right door + ("CF_Gway_TurnSigLh", "CGW1"), + ("CF_Gway_TurnSigRh", "CGW1"), + ("CF_Gway_ParkBrakeSw", "CGW1"), + + ("CYL_PRES", "ESP12"), + + ("CF_Clu_CruiseSwState", "CLU11"), + ("CF_Clu_CruiseSwMain", "CLU11"), + ("CF_Clu_SldMainSW", "CLU11"), + ("CF_Clu_ParityBit1", "CLU11"), + ("CF_Clu_VanzDecimal" , "CLU11"), + ("CF_Clu_Vanz", "CLU11"), + ("CF_Clu_SPEED_UNIT", "CLU11"), + ("CF_Clu_DetentOut", "CLU11"), + ("CF_Clu_RheostatLevel", "CLU11"), + ("CF_Clu_CluInfo", "CLU11"), + ("CF_Clu_AmpInfo", "CLU11"), + ("CF_Clu_AliveCnt1", "CLU11"), + + ("ACCEnable", "TCS13"), + ("ACC_REQ", "TCS13"), + ("DriverBraking", "TCS13"), + ("StandStill", "TCS13"), + ("PBRAKE_ACT", "TCS13"), + + ("ESC_Off_Step", "TCS15"), + ("AVH_LAMP", "TCS15"), + + ("CR_Mdps_StrColTq", "MDPS12"), + ("CF_Mdps_ToiActive", "MDPS12"), + ("CF_Mdps_ToiUnavail", "MDPS12"), + ("CF_Mdps_ToiFlt", "MDPS12"), + ("CR_Mdps_OutTq", "MDPS12"), + + ("SAS_Angle", "SAS11"), + ("SAS_Speed", "SAS11"), ] checks = [ @@ -189,11 +189,11 @@ def get_can_parser(CP): if not CP.openpilotLongitudinalControl: signals += [ - ("MainMode_ACC", "SCC11", 0), - ("VSetDis", "SCC11", 0), - ("SCCInfoDisplay", "SCC11", 0), - ("ACC_ObjDist", "SCC11", 0), - ("ACCMode", "SCC12", 1), + ("MainMode_ACC", "SCC11"), + ("VSetDis", "SCC11"), + ("SCCInfoDisplay", "SCC11"), + ("ACC_ObjDist", "SCC11"), + ("ACCMode", "SCC12"), ] checks += [ @@ -203,39 +203,33 @@ def get_can_parser(CP): if CP.carFingerprint in FEATURES["use_fca"]: signals += [ - ("FCA_CmdAct", "FCA11", 0), - ("CF_VSM_Warn", "FCA11", 0), + ("FCA_CmdAct", "FCA11"), + ("CF_VSM_Warn", "FCA11"), ] - checks += [("FCA11", 50)] + checks.append(("FCA11", 50)) else: signals += [ - ("AEB_CmdAct", "SCC12", 0), - ("CF_VSM_Warn", "SCC12", 0), + ("AEB_CmdAct", "SCC12"), + ("CF_VSM_Warn", "SCC12"), ] if CP.enableBsm: signals += [ - ("CF_Lca_IndLeft", "LCA11", 0), - ("CF_Lca_IndRight", "LCA11", 0), + ("CF_Lca_IndLeft", "LCA11"), + ("CF_Lca_IndRight", "LCA11"), ] - checks += [("LCA11", 50)] + checks.append(("LCA11", 50)) if CP.carFingerprint in (HYBRID_CAR | EV_CAR): if CP.carFingerprint in HYBRID_CAR: - signals += [ - ("CR_Vcu_AccPedDep_Pos", "E_EMS11", 0) - ] + signals.append(("CR_Vcu_AccPedDep_Pos", "E_EMS11")) else: - signals += [ - ("Accel_Pedal_Pos", "E_EMS11", 0) - ] - checks += [ - ("E_EMS11", 50), - ] + signals.append(("Accel_Pedal_Pos", "E_EMS11")) + checks.append(("E_EMS11", 50)) else: signals += [ - ("PV_AV_CAN", "EMS12", 0), - ("CF_Ems_AclAct", "EMS16", 0), + ("PV_AV_CAN", "EMS12"), + ("CF_Ems_AclAct", "EMS16"), ] checks += [ ("EMS12", 100), @@ -243,52 +237,39 @@ def get_can_parser(CP): ] if CP.carFingerprint in FEATURES["use_cluster_gears"]: - signals += [ - ("CF_Clu_Gear", "CLU15", 0), - ] - checks += [ - ("CLU15", 5) - ] + signals.append(("CF_Clu_Gear", "CLU15")) + checks.append(("CLU15", 5)) elif CP.carFingerprint in FEATURES["use_tcu_gears"]: - signals += [ - ("CUR_GR", "TCU12", 0) - ] - checks += [ - ("TCU12", 100) - ] + signals.append(("CUR_GR", "TCU12")) + checks.append(("TCU12", 100)) elif CP.carFingerprint in FEATURES["use_elect_gears"]: - signals += [("Elect_Gear_Shifter", "ELECT_GEAR", 0)] - checks += [("ELECT_GEAR", 20)] + signals.append(("Elect_Gear_Shifter", "ELECT_GEAR")) + checks.append(("ELECT_GEAR", 20)) else: - signals += [ - ("CF_Lvr_Gear", "LVR12", 0) - ] - checks += [ - ("LVR12", 100) - ] + signals.append(("CF_Lvr_Gear", "LVR12")) + checks.append(("LVR12", 100)) return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 0) @staticmethod def get_cam_can_parser(CP): - signals = [ - # sig_name, sig_address, default - ("CF_Lkas_LdwsActivemode", "LKAS11", 0), - ("CF_Lkas_LdwsSysState", "LKAS11", 0), - ("CF_Lkas_SysWarning", "LKAS11", 0), - ("CF_Lkas_LdwsLHWarning", "LKAS11", 0), - ("CF_Lkas_LdwsRHWarning", "LKAS11", 0), - ("CF_Lkas_HbaLamp", "LKAS11", 0), - ("CF_Lkas_FcwBasReq", "LKAS11", 0), - ("CF_Lkas_HbaSysState", "LKAS11", 0), - ("CF_Lkas_FcwOpt", "LKAS11", 0), - ("CF_Lkas_HbaOpt", "LKAS11", 0), - ("CF_Lkas_FcwSysState", "LKAS11", 0), - ("CF_Lkas_FcwCollisionWarning", "LKAS11", 0), - ("CF_Lkas_FusionState", "LKAS11", 0), - ("CF_Lkas_FcwOpt_USM", "LKAS11", 0), - ("CF_Lkas_LdwsOpt_USM", "LKAS11", 0), + # sig_name, sig_address + ("CF_Lkas_LdwsActivemode", "LKAS11"), + ("CF_Lkas_LdwsSysState", "LKAS11"), + ("CF_Lkas_SysWarning", "LKAS11"), + ("CF_Lkas_LdwsLHWarning", "LKAS11"), + ("CF_Lkas_LdwsRHWarning", "LKAS11"), + ("CF_Lkas_HbaLamp", "LKAS11"), + ("CF_Lkas_FcwBasReq", "LKAS11"), + ("CF_Lkas_HbaSysState", "LKAS11"), + ("CF_Lkas_FcwOpt", "LKAS11"), + ("CF_Lkas_HbaOpt", "LKAS11"), + ("CF_Lkas_FcwSysState", "LKAS11"), + ("CF_Lkas_FcwCollisionWarning", "LKAS11"), + ("CF_Lkas_FusionState", "LKAS11"), + ("CF_Lkas_FcwOpt_USM", "LKAS11"), + ("CF_Lkas_LdwsOpt_USM", "LKAS11"), ] checks = [ diff --git a/selfdrive/car/hyundai/hyundaican.py b/selfdrive/car/hyundai/hyundaican.py index c038ff4e9e4049..fd3fc78e88cc8c 100644 --- a/selfdrive/car/hyundai/hyundaican.py +++ b/selfdrive/car/hyundai/hyundaican.py @@ -16,10 +16,10 @@ def create_lkas11(packer, frame, car_fingerprint, apply_steer, steer_req, values["CF_Lkas_ActToi"] = steer_req values["CF_Lkas_MsgCount"] = frame % 0x10 - if car_fingerprint in [CAR.SONATA, CAR.PALISADE, CAR.KIA_NIRO_EV, CAR.KIA_NIRO_HEV_2021, CAR.SANTA_FE, + if car_fingerprint in (CAR.SONATA, CAR.PALISADE, CAR.KIA_NIRO_EV, CAR.KIA_NIRO_HEV_2021, CAR.SANTA_FE, CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV, CAR.KIA_SELTOS, CAR.ELANTRA_2021, CAR.GENESIS_G70_2020, CAR.ELANTRA_HEV_2021, CAR.SONATA_HYBRID, CAR.KONA_EV, CAR.KONA_HEV, CAR.SANTA_FE_2022, - CAR.KIA_K5_2021, CAR.IONIQ_HEV_2022, CAR.SANTA_FE_HEV_2022, CAR.SANTA_FE_PHEV_2022]: + CAR.KIA_K5_2021, CAR.IONIQ_HEV_2022, CAR.SANTA_FE_HEV_2022, CAR.SANTA_FE_PHEV_2022): values["CF_Lkas_LdwsActivemode"] = int(left_lane) + (int(right_lane) << 1) values["CF_Lkas_LdwsOpt_USM"] = 2 diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index 1a5f36baf132b7..463ff3569adcd8 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -44,7 +44,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[]): # py ret.longitudinalActuatorDelayUpperBound = 1.0 # s - if candidate in [CAR.SANTA_FE, CAR.SANTA_FE_2022, CAR.SANTA_FE_HEV_2022, CAR.SANTA_FE_PHEV_2022]: + if candidate in (CAR.SANTA_FE, CAR.SANTA_FE_2022, CAR.SANTA_FE_HEV_2022, CAR.SANTA_FE_PHEV_2022): ret.lateralTuning.pid.kf = 0.00005 ret.mass = 3982. * CV.LB_TO_KG + STD_CARGO_KG ret.wheelbase = 2.766 @@ -53,7 +53,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[]): # py tire_stiffness_factor = 0.82 ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[9., 22.], [9., 22.]] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2, 0.35], [0.05, 0.09]] - elif candidate in [CAR.SONATA, CAR.SONATA_HYBRID]: + elif candidate in (CAR.SONATA, CAR.SONATA_HYBRID): ret.lateralTuning.pid.kf = 0.00005 ret.mass = 1513. + STD_CARGO_KG ret.wheelbase = 2.84 @@ -76,7 +76,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[]): # py tire_stiffness_factor = 0.63 ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.3], [0.05]] - elif candidate in [CAR.ELANTRA, CAR.ELANTRA_GT_I30]: + elif candidate in (CAR.ELANTRA, CAR.ELANTRA_GT_I30): ret.lateralTuning.pid.kf = 0.00006 ret.mass = 1275. + STD_CARGO_KG ret.wheelbase = 2.7 @@ -116,7 +116,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[]): # py ret.lateralTuning.indi.actuatorEffectivenessBP = [0.] ret.lateralTuning.indi.actuatorEffectivenessV = [2.3] ret.minSteerSpeed = 60 * CV.KPH_TO_MS - elif candidate in [CAR.KONA, CAR.KONA_EV, CAR.KONA_HEV]: + elif candidate in (CAR.KONA, CAR.KONA_EV, CAR.KONA_HEV): ret.lateralTuning.pid.kf = 0.00005 ret.mass = {CAR.KONA_EV: 1685., CAR.KONA_HEV: 1425.}.get(candidate, 1275.) + STD_CARGO_KG ret.wheelbase = 2.7 @@ -124,7 +124,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[]): # py tire_stiffness_factor = 0.385 ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]] - elif candidate in [CAR.IONIQ, CAR.IONIQ_EV_LTD, CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV, CAR.IONIQ_HEV_2022]: + elif candidate in (CAR.IONIQ, CAR.IONIQ_EV_LTD, CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV, CAR.IONIQ_HEV_2022): ret.lateralTuning.pid.kf = 0.00006 ret.mass = 1490. + STD_CARGO_KG # weight per hyundai site https://www.hyundaiusa.com/ioniq-electric/specifications.aspx ret.wheelbase = 2.7 @@ -132,7 +132,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[]): # py tire_stiffness_factor = 0.385 ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]] - if candidate not in [CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV, CAR.IONIQ_HEV_2022]: + if candidate not in (CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV, CAR.IONIQ_HEV_2022): ret.minSteerSpeed = 32 * CV.MPH_TO_MS elif candidate == CAR.VELOSTER: ret.lateralTuning.pid.kf = 0.00005 @@ -151,7 +151,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[]): # py ret.steerRatio = 14.4 * 1.1 # 10% higher at the center seems reasonable ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]] - elif candidate in [CAR.KIA_NIRO_EV, CAR.KIA_NIRO_HEV, CAR.KIA_NIRO_HEV_2021]: + elif candidate in (CAR.KIA_NIRO_EV, CAR.KIA_NIRO_HEV, CAR.KIA_NIRO_HEV_2021): ret.lateralTuning.pid.kf = 0.00006 ret.mass = 1737. + STD_CARGO_KG ret.wheelbase = 2.7 @@ -175,7 +175,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[]): # py ret.lateralTuning.indi.timeConstantV = [1.4] ret.lateralTuning.indi.actuatorEffectivenessBP = [0.] ret.lateralTuning.indi.actuatorEffectivenessV = [1.8] - elif candidate in [CAR.KIA_OPTIMA, CAR.KIA_OPTIMA_H]: + elif candidate in (CAR.KIA_OPTIMA, CAR.KIA_OPTIMA_H): ret.lateralTuning.pid.kf = 0.00005 ret.mass = 3558. * CV.LB_TO_KG ret.wheelbase = 2.80 @@ -326,7 +326,7 @@ def update(self, c, can_strings): for b in ret.buttonEvents: # do enable on both accel and decel buttons - if b.type in [ButtonType.accelCruise, ButtonType.decelCruise] and not b.pressed: + if b.type in (ButtonType.accelCruise, ButtonType.decelCruise) and not b.pressed: events.add(EventName.buttonEnable) # do disable on button down if b.type == ButtonType.cancel and b.pressed: @@ -347,7 +347,7 @@ def update(self, c, can_strings): def apply(self, c): hud_control = c.hudControl - ret = self.CC.update(c.enabled, self.CS, self.frame, c.actuators, + ret = self.CC.update(c, c.enabled, self.CS, self.frame, c.actuators, c.cruiseControl.cancel, hud_control.visualAlert, hud_control.setSpeed, hud_control.leftLaneVisible, hud_control.rightLaneVisible, hud_control.leftLaneDepart, hud_control.rightLaneDepart) self.frame += 1 diff --git a/selfdrive/car/hyundai/radar_interface.py b/selfdrive/car/hyundai/radar_interface.py index 6022069a2d6661..0d22611fb577ce 100644 --- a/selfdrive/car/hyundai/radar_interface.py +++ b/selfdrive/car/hyundai/radar_interface.py @@ -20,11 +20,11 @@ def get_radar_can_parser(CP): for addr in range(RADAR_START_ADDR, RADAR_START_ADDR + RADAR_MSG_COUNT): msg = f"RADAR_TRACK_{addr:x}" signals += [ - ("STATE", msg, 0), - ("AZIMUTH", msg, 0), - ("LONG_DIST", msg, 0), - ("REL_ACCEL", msg, 0), - ("REL_SPEED", msg, 0), + ("STATE", msg), + ("AZIMUTH", msg), + ("LONG_DIST", msg), + ("REL_ACCEL", msg), + ("REL_SPEED", msg), ] checks += [(msg, 50)] return CANParser(DBC[CP.carFingerprint]['radar'], signals, checks, 1) @@ -74,14 +74,14 @@ def _update(self, updated_messages): self.pts[addr].trackId = self.track_id self.track_id += 1 - valid = msg['STATE'] in [3, 4] + valid = msg['STATE'] in (3, 4) if valid: azimuth = math.radians(msg['AZIMUTH']) self.pts[addr].measured = True self.pts[addr].dRel = math.cos(azimuth) * msg['LONG_DIST'] self.pts[addr].yRel = 0.5 * -math.sin(azimuth) * msg['LONG_DIST'] self.pts[addr].vRel = msg['REL_SPEED'] - self.pts[addr].aRel = msg['REL_ACCEL'] + self.pts[addr].aRel = msg['REL_ACCEL'] self.pts[addr].yvRel = float('nan') else: diff --git a/selfdrive/car/hyundai/values.py b/selfdrive/car/hyundai/values.py index 97d273d9438093..d940144f868673 100644 --- a/selfdrive/car/hyundai/values.py +++ b/selfdrive/car/hyundai/values.py @@ -1,5 +1,3 @@ -# flake8: noqa - from cereal import car from selfdrive.car import dbc_dict Ecu = car.CarParams.Ecu @@ -12,9 +10,9 @@ class CarControllerParams: def __init__(self, CP): # To determine the limit for your car, find the maximum value that the stock LKAS will request. # If the max stock LKAS request is <384, add your car to this list. - if CP.carFingerprint in [CAR.GENESIS_G80, CAR.GENESIS_G90, CAR.ELANTRA, CAR.HYUNDAI_GENESIS, CAR.ELANTRA_GT_I30, CAR.IONIQ, + if CP.carFingerprint in (CAR.GENESIS_G80, CAR.GENESIS_G90, CAR.ELANTRA, CAR.HYUNDAI_GENESIS, CAR.ELANTRA_GT_I30, CAR.IONIQ, CAR.IONIQ_EV_LTD, CAR.IONIQ_PHEV, CAR.SANTA_FE_PHEV_2022, CAR.SONATA_LF, CAR.KIA_FORTE, CAR.KIA_NIRO_HEV, - CAR.KIA_NIRO_HEV_2021, CAR.KIA_OPTIMA_H, CAR.KIA_OPTIMA, CAR.KIA_SORENTO, CAR.KIA_STINGER]: + CAR.KIA_NIRO_HEV_2021, CAR.KIA_OPTIMA_H, CAR.KIA_OPTIMA, CAR.KIA_SORENTO, CAR.KIA_STINGER): self.STEER_MAX = 255 else: self.STEER_MAX = 384 @@ -672,7 +670,7 @@ class Buttons: b'\xf1\x00IK MDPS R 1.00 1.07 57700-G9220 4I2VL107', ], (Ecu.transmission, 0x7e1, None): [ - b'\xf1\x87VCJLP18407832DN3\x88vXfvUVT\x97eFU\x87d7v\x88eVeveFU\x89\x98\x7f\xff\xb2\xb0\xf1\x81E25\x00\x00\x00' + b'\xf1\x87VCJLP18407832DN3\x88vXfvUVT\x97eFU\x87d7v\x88eVeveFU\x89\x98\x7f\xff\xb2\xb0\xf1\x81E25\x00\x00\x00', b'\x00\x00\x00\x00\xf1\x00bcsh8p54 E25\x00\x00\x00\x00\x00\x00\x00SIK0T33NB4\xecE\xefL', ], (Ecu.fwdRadar, 0x7d0, None): [ @@ -798,7 +796,7 @@ class Buttons: b'\xf1\x00DEev SCC F-CUP 1.00 1.03 96400-Q4100 ', b'\xf1\x8799110Q4000\xf1\x00DEev SCC F-CUP 1.00 1.00 99110-Q4000 ', b'\xf1\x8799110Q4100\xf1\x00DEev SCC F-CUP 1.00 1.00 99110-Q4100 ', - b'\xf1\x8799110Q4500\xf1\000DEev SCC F-CUP 1.00 1.00 99110-Q4500 ', + b'\xf1\x8799110Q4500\xf1\x00DEev SCC F-CUP 1.00 1.00 99110-Q4500 ', b'\xf1\x8799110Q4600\xf1\x00DEev SCC FNCUP 1.00 1.00 99110-Q4600 ', b'\xf1\x8799110Q4600\xf1\x00DEev SCC FHCUP 1.00 1.00 99110-Q4600 ', ], @@ -807,7 +805,7 @@ class Buttons: b'\xf1\x00DE MDPS C 1.00 1.05 56310Q4100\x00 4DEEC105', ], (Ecu.fwdCamera, 0x7C4, None): [ - b'\xf1\000DEE MFC AT EUR LHD 1.00 1.00 99211-Q4100 200706', + b'\xf1\x00DEE MFC AT EUR LHD 1.00 1.00 99211-Q4100 200706', b'\xf1\x00DEE MFC AT EUR LHD 1.00 1.00 99211-Q4000 191211', b'\xf1\x00DEE MFC AT USA LHD 1.00 1.00 99211-Q4000 191211', b'\xf1\x00DEE MFC AT USA LHD 1.00 1.03 95740-Q4000 180821', @@ -1015,7 +1013,7 @@ class Buttons: EV_CAR = {CAR.IONIQ_EV_2020, CAR.IONIQ_EV_LTD, CAR.KONA_EV, CAR.KIA_NIRO_EV} # these cars require a special panda safety mode due to missing counters and checksums in the messages -LEGACY_SAFETY_MODE_CAR = {CAR.HYUNDAI_GENESIS, CAR.IONIQ_EV_2020, CAR.IONIQ_EV_LTD, CAR.IONIQ_PHEV, CAR.IONIQ, CAR.KONA_EV, CAR.KIA_SORENTO, CAR.SONATA_LF, CAR.KIA_NIRO_EV, CAR.KIA_OPTIMA, CAR.VELOSTER, CAR.KIA_STINGER, CAR.GENESIS_G70, CAR.GENESIS_G80, CAR.KIA_CEED, CAR.ELANTRA, CAR.IONIQ_HEV_2022} +LEGACY_SAFETY_MODE_CAR = {CAR.HYUNDAI_GENESIS, CAR.IONIQ_EV_2020, CAR.IONIQ_EV_LTD, CAR.IONIQ_PHEV, CAR.IONIQ, CAR.KONA_EV, CAR.KIA_SORENTO, CAR.SONATA_LF, CAR.KIA_OPTIMA, CAR.VELOSTER, CAR.KIA_STINGER, CAR.GENESIS_G70, CAR.GENESIS_G80, CAR.KIA_CEED, CAR.ELANTRA, CAR.IONIQ_HEV_2022} # If 0x500 is present on bus 1 it probably has a Mando radar outputting radar points. # If no points are outputted by default it might be possible to turn it on using selfdrive/debug/hyundai_enable_radar_points.py @@ -1036,7 +1034,7 @@ class Buttons: CAR.IONIQ_HEV_2022: dbc_dict('hyundai_kia_generic', None), CAR.KIA_FORTE: dbc_dict('hyundai_kia_generic', None), CAR.KIA_K5_2021: dbc_dict('hyundai_kia_generic', None), - CAR.KIA_NIRO_EV: dbc_dict('hyundai_kia_generic', None), + CAR.KIA_NIRO_EV: dbc_dict('hyundai_kia_generic', 'hyundai_kia_mando_front_radar'), CAR.KIA_NIRO_HEV: dbc_dict('hyundai_kia_generic', 'hyundai_kia_mando_front_radar'), CAR.KIA_NIRO_HEV_2021: dbc_dict('hyundai_kia_generic', None), CAR.KIA_OPTIMA: dbc_dict('hyundai_kia_generic', None), diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index da62980e75ac7e..ccef8950b4dd23 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -74,6 +74,7 @@ def get_steer_feedforward_function(cls): def get_std_params(candidate, fingerprint): ret = car.CarParams.new_message() ret.carFingerprint = candidate + ret.unsafeMode = 1 # see safety_declarations.h for allowed values # standard ALC params ret.steerControlType = car.CarParams.SteerControlType.torque @@ -99,6 +100,7 @@ def get_std_params(candidate, fingerprint): ret.longitudinalTuning.kiV = [1.] ret.longitudinalActuatorDelayLowerBound = 0.15 ret.longitudinalActuatorDelayUpperBound = 0.15 + ret.steerLimitTimer = 1.0 return ret @abstractmethod @@ -109,7 +111,7 @@ def update(self, c: car.CarControl, can_strings: List[bytes]) -> car.CarState: def apply(self, c: car.CarControl) -> Tuple[car.CarControl.Actuators, List[bytes]]: pass - def create_common_events(self, cs_out, extra_gears=None, gas_resume_speed=-1, pcm_enable=True): + def create_common_events(self, cs_out, extra_gears=None, pcm_enable=True): events = Events() if cs_out.doorOpen: @@ -154,9 +156,7 @@ def create_common_events(self, cs_out, extra_gears=None, gas_resume_speed=-1, pc events.add(EventName.steerUnavailable) # Disable on rising edge of gas or brake. Also disable on brake when speed > 0. - # Optionally allow to press gas at zero speed to resume. - # e.g. Chrysler does not spam the resume button yet, so resuming with gas is handy. FIXME! - if (self.op_params.get('disengage_on_gas') and cs_out.gasPressed and (not self.CS.out.gasPressed) and cs_out.vEgo > gas_resume_speed) or \ + if (self.op_params.get('disengage_on_gas') and cs_out.gasPressed and not self.CS.out.gasPressed) or \ (cs_out.brakePressed and (not self.CS.out.brakePressed or not cs_out.standstill)): events.add(EventName.pedalPressed) diff --git a/selfdrive/car/mazda/carcontroller.py b/selfdrive/car/mazda/carcontroller.py index c65ff72ed52248..60bb62037753dc 100644 --- a/selfdrive/car/mazda/carcontroller.py +++ b/selfdrive/car/mazda/carcontroller.py @@ -19,7 +19,7 @@ def update(self, c, CS, frame): apply_steer = 0 self.steer_rate_limited = False - if c.enabled: + if c.active: # calculate steer and also set limits due to driver torque new_steer = int(round(c.actuators.steer * CarControllerParams.STEER_MAX)) apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, diff --git a/selfdrive/car/mazda/carstate.py b/selfdrive/car/mazda/carstate.py index 7ebf60c96bd6ea..8b9fd4f3901b40 100644 --- a/selfdrive/car/mazda/carstate.py +++ b/selfdrive/car/mazda/carstate.py @@ -16,6 +16,7 @@ def __init__(self, CP): self.acc_active_last = False self.low_speed_alert = False self.lkas_allowed_speed = False + self.lkas_disabled = False def update(self, cp, cp_cam): @@ -91,29 +92,31 @@ def update(self, cp, cp_cam): self.acc_active_last = ret.cruiseState.enabled + self.crz_btns_counter = cp.vl["CRZ_BTNS"]["CTR"] + + # camera signals + self.lkas_disabled = cp_cam.vl["CAM_LANEINFO"]["LANE_LINES"] == 0 self.cam_lkas = cp_cam.vl["CAM_LKAS"] self.cam_laneinfo = cp_cam.vl["CAM_LANEINFO"] - self.crz_btns_counter = cp.vl["CRZ_BTNS"]["CTR"] ret.steerError = cp_cam.vl["CAM_LKAS"]["ERR_BIT_1"] == 1 return ret @staticmethod def get_can_parser(CP): - # this function generates lists for signal, messages and initial values signals = [ - # sig_name, sig_address, default - ("LEFT_BLINK", "BLINK_INFO", 0), - ("RIGHT_BLINK", "BLINK_INFO", 0), - ("HIGH_BEAMS", "BLINK_INFO", 0), - ("STEER_ANGLE", "STEER", 0), - ("STEER_ANGLE_RATE", "STEER_RATE", 0), - ("STEER_TORQUE_SENSOR", "STEER_TORQUE", 0), - ("STEER_TORQUE_MOTOR", "STEER_TORQUE", 0), - ("FL", "WHEEL_SPEEDS", 0), - ("FR", "WHEEL_SPEEDS", 0), - ("RL", "WHEEL_SPEEDS", 0), - ("RR", "WHEEL_SPEEDS", 0), + # sig_name, sig_address + ("LEFT_BLINK", "BLINK_INFO"), + ("RIGHT_BLINK", "BLINK_INFO"), + ("HIGH_BEAMS", "BLINK_INFO"), + ("STEER_ANGLE", "STEER"), + ("STEER_ANGLE_RATE", "STEER_RATE"), + ("STEER_TORQUE_SENSOR", "STEER_TORQUE"), + ("STEER_TORQUE_MOTOR", "STEER_TORQUE"), + ("FL", "WHEEL_SPEEDS"), + ("FR", "WHEEL_SPEEDS"), + ("RL", "WHEEL_SPEEDS"), + ("RR", "WHEEL_SPEEDS"), ] checks = [ @@ -127,26 +130,26 @@ def get_can_parser(CP): if CP.carFingerprint in GEN1: signals += [ - ("LKAS_BLOCK", "STEER_RATE", 0), - ("LKAS_TRACK_STATE", "STEER_RATE", 0), - ("HANDS_OFF_5_SECONDS", "STEER_RATE", 0), - ("CRZ_ACTIVE", "CRZ_CTRL", 0), - ("CRZ_AVAILABLE", "CRZ_CTRL", 0), - ("CRZ_SPEED", "CRZ_EVENTS", 0), - ("STANDSTILL", "PEDALS", 0), - ("BRAKE_ON", "PEDALS", 0), - ("BRAKE_PRESSURE", "BRAKE", 0), - ("GEAR", "GEAR", 0), - ("DRIVER_SEATBELT", "SEATBELT", 0), - ("FL", "DOORS", 0), - ("FR", "DOORS", 0), - ("BL", "DOORS", 0), - ("BR", "DOORS", 0), - ("PEDAL_GAS", "ENGINE_DATA", 0), - ("SPEED", "ENGINE_DATA", 0), - ("CTR", "CRZ_BTNS", 0), - ("LEFT_BS1", "BSM", 0), - ("RIGHT_BS1", "BSM", 0), + ("LKAS_BLOCK", "STEER_RATE"), + ("LKAS_TRACK_STATE", "STEER_RATE"), + ("HANDS_OFF_5_SECONDS", "STEER_RATE"), + ("CRZ_ACTIVE", "CRZ_CTRL"), + ("CRZ_AVAILABLE", "CRZ_CTRL"), + ("CRZ_SPEED", "CRZ_EVENTS"), + ("STANDSTILL", "PEDALS"), + ("BRAKE_ON", "PEDALS"), + ("BRAKE_PRESSURE", "BRAKE"), + ("GEAR", "GEAR"), + ("DRIVER_SEATBELT", "SEATBELT"), + ("FL", "DOORS"), + ("FR", "DOORS"), + ("BL", "DOORS"), + ("BR", "DOORS"), + ("PEDAL_GAS", "ENGINE_DATA"), + ("SPEED", "ENGINE_DATA"), + ("CTR", "CRZ_BTNS"), + ("LEFT_BS1", "BSM"), + ("RIGHT_BS1", "BSM"), ] checks += [ @@ -171,26 +174,26 @@ def get_cam_can_parser(CP): if CP.carFingerprint in GEN1: signals += [ - # sig_name, sig_address, default - ("LKAS_REQUEST", "CAM_LKAS", 0), - ("CTR", "CAM_LKAS", 0), - ("ERR_BIT_1", "CAM_LKAS", 0), - ("LINE_NOT_VISIBLE", "CAM_LKAS", 0), - ("BIT_1", "CAM_LKAS", 1), - ("ERR_BIT_2", "CAM_LKAS", 0), - ("STEERING_ANGLE", "CAM_LKAS", 0), - ("ANGLE_ENABLED", "CAM_LKAS", 0), - ("CHKSUM", "CAM_LKAS", 0), - - ("LINE_VISIBLE", "CAM_LANEINFO", 0), - ("LINE_NOT_VISIBLE", "CAM_LANEINFO", 1), - ("LANE_LINES", "CAM_LANEINFO", 0), - ("BIT1", "CAM_LANEINFO", 0), - ("BIT2", "CAM_LANEINFO", 0), - ("BIT3", "CAM_LANEINFO", 0), - ("NO_ERR_BIT", "CAM_LANEINFO", 1), - ("S1", "CAM_LANEINFO", 0), - ("S1_HBEAM", "CAM_LANEINFO", 0), + # sig_name, sig_address + ("LKAS_REQUEST", "CAM_LKAS"), + ("CTR", "CAM_LKAS"), + ("ERR_BIT_1", "CAM_LKAS"), + ("LINE_NOT_VISIBLE", "CAM_LKAS"), + ("BIT_1", "CAM_LKAS"), + ("ERR_BIT_2", "CAM_LKAS"), + ("STEERING_ANGLE", "CAM_LKAS"), + ("ANGLE_ENABLED", "CAM_LKAS"), + ("CHKSUM", "CAM_LKAS"), + + ("LINE_VISIBLE", "CAM_LANEINFO"), + ("LINE_NOT_VISIBLE", "CAM_LANEINFO"), + ("LANE_LINES", "CAM_LANEINFO"), + ("BIT1", "CAM_LANEINFO"), + ("BIT2", "CAM_LANEINFO"), + ("BIT3", "CAM_LANEINFO"), + ("NO_ERR_BIT", "CAM_LANEINFO"), + ("S1", "CAM_LANEINFO"), + ("S1_HBEAM", "CAM_LANEINFO"), ] checks += [ diff --git a/selfdrive/car/mazda/interface.py b/selfdrive/car/mazda/interface.py index b4ae938228b885..b703402cd9d28e 100755 --- a/selfdrive/car/mazda/interface.py +++ b/selfdrive/car/mazda/interface.py @@ -22,7 +22,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None): ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.mazda)] ret.radarOffCan = True - ret.dashcamOnly = candidate not in [CAR.CX9_2021] + ret.dashcamOnly = candidate not in (CAR.CX9_2021,) ret.steerActuatorDelay = 0.1 ret.steerRateCost = 1.0 @@ -36,7 +36,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None): ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.19], [0.019]] ret.lateralTuning.pid.kf = 0.00006 - elif candidate in [CAR.CX9, CAR.CX9_2021]: + elif candidate in (CAR.CX9, CAR.CX9_2021): ret.mass = 4217 * CV.LB_TO_KG + STD_CARGO_KG ret.wheelbase = 3.1 ret.steerRatio = 17.6 @@ -86,7 +86,9 @@ def update(self, c, can_strings): # events events = self.create_common_events(ret) - if self.CS.low_speed_alert: + if self.CS.lkas_disabled: + events.add(EventName.lkasDisabled) + elif self.CS.low_speed_alert: events.add(EventName.belowSteerSpeed) ret.events = events.to_msg() diff --git a/selfdrive/car/mazda/values.py b/selfdrive/car/mazda/values.py index f18c30176c4c7e..c3a5de0c19d9d0 100644 --- a/selfdrive/car/mazda/values.py +++ b/selfdrive/car/mazda/values.py @@ -1,5 +1,3 @@ -# flake8: noqa - from selfdrive.car import dbc_dict from cereal import car Ecu = car.CarParams.Ecu diff --git a/selfdrive/car/nissan/carcontroller.py b/selfdrive/car/nissan/carcontroller.py index 8b40a050c19efa..8b30c112492a74 100644 --- a/selfdrive/car/nissan/carcontroller.py +++ b/selfdrive/car/nissan/carcontroller.py @@ -18,22 +18,20 @@ def __init__(self, dbc_name, CP, VM): self.packer = CANPacker(dbc_name) - def update(self, enabled, CS, frame, actuators, cruise_cancel, hud_alert, + def update(self, c, enabled, CS, frame, actuators, cruise_cancel, hud_alert, left_line, right_line, left_lane_depart, right_lane_depart): - """ Controls thread """ - # Send CAN commands. can_sends = [] ### STEER ### - acc_active = bool(CS.out.cruiseState.enabled) + acc_active = CS.out.cruiseState.enabled lkas_hud_msg = CS.lkas_hud_msg lkas_hud_info_msg = CS.lkas_hud_info_msg apply_angle = actuators.steeringAngleDeg - steer_hud_alert = 1 if hud_alert in [VisualAlert.steerRequired, VisualAlert.ldw] else 0 + steer_hud_alert = 1 if hud_alert in (VisualAlert.steerRequired, VisualAlert.ldw) else 0 - if enabled: + if c.active: # # windup slower if self.last_angle * apply_angle > 0. and abs(apply_angle) > abs(self.last_angle): angle_rate_lim = interp(CS.out.vEgo, CarControllerParams.ANGLE_DELTA_BP, CarControllerParams.ANGLE_DELTA_V) @@ -64,14 +62,14 @@ def update(self, enabled, CS, frame, actuators, cruise_cancel, hud_alert, # send acc cancel cmd if drive is disabled but pcm is still on, or if the system can't be activated cruise_cancel = 1 - if self.CP.carFingerprint in [CAR.ROGUE, CAR.XTRAIL, CAR.ALTIMA] and cruise_cancel: + if self.CP.carFingerprint in (CAR.ROGUE, CAR.XTRAIL, CAR.ALTIMA) and cruise_cancel: can_sends.append(nissancan.create_acc_cancel_cmd(self.packer, self.car_fingerprint, CS.cruise_throttle_msg, frame)) # TODO: Find better way to cancel! # For some reason spamming the cancel button is unreliable on the Leaf # We now cancel by making propilot think the seatbelt is unlatched, # this generates a beep and a warning message every time you disengage - if self.CP.carFingerprint in [CAR.LEAF, CAR.LEAF_IC] and frame % 2 == 0: + if self.CP.carFingerprint in (CAR.LEAF, CAR.LEAF_IC) and frame % 2 == 0: can_sends.append(nissancan.create_cancel_msg(self.packer, CS.cancel_msg, cruise_cancel)) can_sends.append(nissancan.create_steering_control( diff --git a/selfdrive/car/nissan/carstate.py b/selfdrive/car/nissan/carstate.py index 05191feedf9b99..6b030e9b45d9fc 100644 --- a/selfdrive/car/nissan/carstate.py +++ b/selfdrive/car/nissan/carstate.py @@ -23,16 +23,16 @@ def __init__(self, CP): def update(self, cp, cp_adas, cp_cam): ret = car.CarState.new_message() - if self.CP.carFingerprint in [CAR.ROGUE, CAR.XTRAIL, CAR.ALTIMA]: + if self.CP.carFingerprint in (CAR.ROGUE, CAR.XTRAIL, CAR.ALTIMA): ret.gas = cp.vl["GAS_PEDAL"]["GAS_PEDAL"] - elif self.CP.carFingerprint in [CAR.LEAF, CAR.LEAF_IC]: + elif self.CP.carFingerprint in (CAR.LEAF, CAR.LEAF_IC): ret.gas = cp.vl["CRUISE_THROTTLE"]["GAS_PEDAL"] ret.gasPressed = bool(ret.gas > 3) - if self.CP.carFingerprint in [CAR.ROGUE, CAR.XTRAIL, CAR.ALTIMA]: + if self.CP.carFingerprint in (CAR.ROGUE, CAR.XTRAIL, CAR.ALTIMA): ret.brakePressed = bool(cp.vl["DOORS_LIGHTS"]["USER_BRAKE_PRESSED"]) - elif self.CP.carFingerprint in [CAR.LEAF, CAR.LEAF_IC]: + elif self.CP.carFingerprint in (CAR.LEAF, CAR.LEAF_IC): ret.brakePressed = bool(cp.vl["CRUISE_THROTTLE"]["USER_BRAKE_PRESSED"]) ret.wheelSpeeds = self.get_wheel_speeds( @@ -51,10 +51,10 @@ def update(self, cp, cp_adas, cp_cam): else: ret.cruiseState.enabled = bool(cp_adas.vl["CRUISE_STATE"]["CRUISE_ENABLED"]) - if self.CP.carFingerprint in [CAR.ROGUE, CAR.XTRAIL]: + if self.CP.carFingerprint in (CAR.ROGUE, CAR.XTRAIL): ret.seatbeltUnlatched = cp.vl["HUD"]["SEATBELT_DRIVER_LATCHED"] == 0 ret.cruiseState.available = bool(cp_cam.vl["PRO_PILOT"]["CRUISE_ON"]) - elif self.CP.carFingerprint in [CAR.LEAF, CAR.LEAF_IC]: + elif self.CP.carFingerprint in (CAR.LEAF, CAR.LEAF_IC): if self.CP.carFingerprint == CAR.LEAF: ret.seatbeltUnlatched = cp.vl["SEATBELT"]["SEATBELT_DRIVER_LATCHED"] == 0 elif self.CP.carFingerprint == CAR.LEAF_IC: @@ -70,7 +70,7 @@ def update(self, cp, cp_adas, cp_cam): speed = cp_adas.vl["PROPILOT_HUD"]["SET_SPEED"] if speed != 255: - if self.CP.carFingerprint in [CAR.LEAF, CAR.LEAF_IC]: + if self.CP.carFingerprint in (CAR.LEAF, CAR.LEAF_IC): conversion = CV.MPH_TO_MS if cp.vl["HUD_SETTINGS"]["SPEED_MPH"] else CV.KPH_TO_MS else: conversion = CV.MPH_TO_MS if cp.vl["HUD"]["SPEED_MPH"] else CV.KPH_TO_MS @@ -108,7 +108,7 @@ def update(self, cp, cp_adas, cp_cam): self.cruise_throttle_msg = copy.copy(cp.vl["CRUISE_THROTTLE"]) - if self.CP.carFingerprint in [CAR.LEAF, CAR.LEAF_IC]: + if self.CP.carFingerprint in (CAR.LEAF, CAR.LEAF_IC): self.cancel_msg = copy.copy(cp.vl["CANCEL_MSG"]) if self.CP.carFingerprint != CAR.ALTIMA: @@ -119,27 +119,26 @@ def update(self, cp, cp_adas, cp_cam): @staticmethod def get_can_parser(CP): - # this function generates lists for signal, messages and initial values signals = [ - # sig_name, sig_address, default - ("WHEEL_SPEED_FL", "WHEEL_SPEEDS_FRONT", 0), - ("WHEEL_SPEED_FR", "WHEEL_SPEEDS_FRONT", 0), - ("WHEEL_SPEED_RL", "WHEEL_SPEEDS_REAR", 0), - ("WHEEL_SPEED_RR", "WHEEL_SPEEDS_REAR", 0), + # sig_name, sig_address + ("WHEEL_SPEED_FL", "WHEEL_SPEEDS_FRONT"), + ("WHEEL_SPEED_FR", "WHEEL_SPEEDS_FRONT"), + ("WHEEL_SPEED_RL", "WHEEL_SPEEDS_REAR"), + ("WHEEL_SPEED_RR", "WHEEL_SPEEDS_REAR"), - ("STEER_ANGLE", "STEER_ANGLE_SENSOR", 0), + ("STEER_ANGLE", "STEER_ANGLE_SENSOR"), - ("DOOR_OPEN_FR", "DOORS_LIGHTS", 1), - ("DOOR_OPEN_FL", "DOORS_LIGHTS", 1), - ("DOOR_OPEN_RR", "DOORS_LIGHTS", 1), - ("DOOR_OPEN_RL", "DOORS_LIGHTS", 1), + ("DOOR_OPEN_FR", "DOORS_LIGHTS"), + ("DOOR_OPEN_FL", "DOORS_LIGHTS"), + ("DOOR_OPEN_RR", "DOORS_LIGHTS"), + ("DOOR_OPEN_RL", "DOORS_LIGHTS"), - ("RIGHT_BLINKER", "LIGHTS", 0), - ("LEFT_BLINKER", "LIGHTS", 0), + ("RIGHT_BLINKER", "LIGHTS"), + ("LEFT_BLINKER", "LIGHTS"), - ("ESP_DISABLED", "ESP", 0), + ("ESP_DISABLED", "ESP"), - ("GEAR_SHIFTER", "GEARBOX", 0), + ("GEAR_SHIFTER", "GEARBOX"), ] checks = [ @@ -153,28 +152,28 @@ def get_can_parser(CP): ("LIGHTS", 10), ] - if CP.carFingerprint in [CAR.ROGUE, CAR.XTRAIL, CAR.ALTIMA]: + if CP.carFingerprint in (CAR.ROGUE, CAR.XTRAIL, CAR.ALTIMA): signals += [ - ("USER_BRAKE_PRESSED", "DOORS_LIGHTS", 1), - - ("GAS_PEDAL", "GAS_PEDAL", 0), - ("SEATBELT_DRIVER_LATCHED", "HUD", 0), - ("SPEED_MPH", "HUD", 0), - - ("PROPILOT_BUTTON", "CRUISE_THROTTLE", 0), - ("CANCEL_BUTTON", "CRUISE_THROTTLE", 0), - ("GAS_PEDAL_INVERTED", "CRUISE_THROTTLE", 0), - ("SET_BUTTON", "CRUISE_THROTTLE", 0), - ("RES_BUTTON", "CRUISE_THROTTLE", 0), - ("FOLLOW_DISTANCE_BUTTON", "CRUISE_THROTTLE", 0), - ("NO_BUTTON_PRESSED", "CRUISE_THROTTLE", 0), - ("GAS_PEDAL", "CRUISE_THROTTLE", 0), - ("USER_BRAKE_PRESSED", "CRUISE_THROTTLE", 0), - ("NEW_SIGNAL_2", "CRUISE_THROTTLE", 0), - ("GAS_PRESSED_INVERTED", "CRUISE_THROTTLE", 0), - ("unsure1", "CRUISE_THROTTLE", 0), - ("unsure2", "CRUISE_THROTTLE", 0), - ("unsure3", "CRUISE_THROTTLE", 0), + ("USER_BRAKE_PRESSED", "DOORS_LIGHTS"), + + ("GAS_PEDAL", "GAS_PEDAL"), + ("SEATBELT_DRIVER_LATCHED", "HUD"), + ("SPEED_MPH", "HUD"), + + ("PROPILOT_BUTTON", "CRUISE_THROTTLE"), + ("CANCEL_BUTTON", "CRUISE_THROTTLE"), + ("GAS_PEDAL_INVERTED", "CRUISE_THROTTLE"), + ("SET_BUTTON", "CRUISE_THROTTLE"), + ("RES_BUTTON", "CRUISE_THROTTLE"), + ("FOLLOW_DISTANCE_BUTTON", "CRUISE_THROTTLE"), + ("NO_BUTTON_PRESSED", "CRUISE_THROTTLE"), + ("GAS_PEDAL", "CRUISE_THROTTLE"), + ("USER_BRAKE_PRESSED", "CRUISE_THROTTLE"), + ("NEW_SIGNAL_2", "CRUISE_THROTTLE"), + ("GAS_PRESSED_INVERTED", "CRUISE_THROTTLE"), + ("unsure1", "CRUISE_THROTTLE"), + ("unsure2", "CRUISE_THROTTLE"), + ("unsure3", "CRUISE_THROTTLE"), ] checks += [ @@ -183,19 +182,19 @@ def get_can_parser(CP): ("HUD", 25), ] - elif CP.carFingerprint in [CAR.LEAF, CAR.LEAF_IC]: + elif CP.carFingerprint in (CAR.LEAF, CAR.LEAF_IC): signals += [ - ("USER_BRAKE_PRESSED", "CRUISE_THROTTLE", 1), - ("GAS_PEDAL", "CRUISE_THROTTLE", 0), - ("CRUISE_AVAILABLE", "CRUISE_THROTTLE", 0), - ("SPEED_MPH", "HUD_SETTINGS", 0), - ("SEATBELT_DRIVER_LATCHED", "SEATBELT", 0), + ("USER_BRAKE_PRESSED", "CRUISE_THROTTLE"), + ("GAS_PEDAL", "CRUISE_THROTTLE"), + ("CRUISE_AVAILABLE", "CRUISE_THROTTLE"), + ("SPEED_MPH", "HUD_SETTINGS"), + ("SEATBELT_DRIVER_LATCHED", "SEATBELT"), # Copy other values, we use this to cancel - ("CANCEL_SEATBELT", "CANCEL_MSG", 0), - ("NEW_SIGNAL_1", "CANCEL_MSG", 0), - ("NEW_SIGNAL_2", "CANCEL_MSG", 0), - ("NEW_SIGNAL_3", "CANCEL_MSG", 0), + ("CANCEL_SEATBELT", "CANCEL_MSG"), + ("NEW_SIGNAL_1", "CANCEL_MSG"), + ("NEW_SIGNAL_2", "CANCEL_MSG"), + ("NEW_SIGNAL_3", "CANCEL_MSG"), ] checks += [ ("BRAKE_PEDAL", 100), @@ -207,9 +206,9 @@ def get_can_parser(CP): if CP.carFingerprint == CAR.ALTIMA: signals += [ - ("LKAS_ENABLED", "LKAS_SETTINGS", 0), - ("CRUISE_ENABLED", "CRUISE_STATE", 0), - ("SET_SPEED", "PROPILOT_HUD", 0), + ("LKAS_ENABLED", "LKAS_SETTINGS"), + ("CRUISE_ENABLED", "CRUISE_STATE"), + ("SET_SPEED", "PROPILOT_HUD"), ] checks += [ ("CRUISE_STATE", 10), @@ -218,12 +217,8 @@ def get_can_parser(CP): ] return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 1) - signals += [ - ("STEER_TORQUE_DRIVER", "STEER_TORQUE_SENSOR", 0), - ] - checks += [ - ("STEER_TORQUE_SENSOR", 100), - ] + signals.append(("STEER_TORQUE_DRIVER", "STEER_TORQUE_SENSOR")) + checks.append(("STEER_TORQUE_SENSOR", 100)) return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 0) @@ -233,14 +228,14 @@ def get_adas_can_parser(CP): if CP.carFingerprint == CAR.ALTIMA: signals = [ - ("DESIRED_ANGLE", "LKAS", 0), - ("SET_0x80_2", "LKAS", 0), - ("MAX_TORQUE", "LKAS", 0), - ("SET_0x80", "LKAS", 0), - ("COUNTER", "LKAS", 0), - ("LKA_ACTIVE", "LKAS", 0), - - ("CRUISE_ON", "PRO_PILOT", 0), + ("DESIRED_ANGLE", "LKAS"), + ("SET_0x80_2", "LKAS"), + ("MAX_TORQUE", "LKAS"), + ("SET_0x80", "LKAS"), + ("COUNTER", "LKAS"), + ("LKA_ACTIVE", "LKAS"), + + ("CRUISE_ON", "PRO_PILOT"), ] checks = [ ("LKAS", 100), @@ -248,85 +243,85 @@ def get_adas_can_parser(CP): ] else: signals = [ - # sig_name, sig_address, default - ("LKAS_ENABLED", "LKAS_SETTINGS", 0), + # sig_name, sig_address + ("LKAS_ENABLED", "LKAS_SETTINGS"), - ("CRUISE_ENABLED", "CRUISE_STATE", 0), + ("CRUISE_ENABLED", "CRUISE_STATE"), - ("DESIRED_ANGLE", "LKAS", 0), - ("SET_0x80_2", "LKAS", 0), - ("MAX_TORQUE", "LKAS", 0), - ("SET_0x80", "LKAS", 0), - ("COUNTER", "LKAS", 0), - ("LKA_ACTIVE", "LKAS", 0), + ("DESIRED_ANGLE", "LKAS"), + ("SET_0x80_2", "LKAS"), + ("MAX_TORQUE", "LKAS"), + ("SET_0x80", "LKAS"), + ("COUNTER", "LKAS"), + ("LKA_ACTIVE", "LKAS"), # Below are the HUD messages. We copy the stock message and modify - ("LARGE_WARNING_FLASHING", "PROPILOT_HUD", 0), - ("SIDE_RADAR_ERROR_FLASHING1", "PROPILOT_HUD", 0), - ("SIDE_RADAR_ERROR_FLASHING2", "PROPILOT_HUD", 0), - ("LEAD_CAR", "PROPILOT_HUD", 0), - ("LEAD_CAR_ERROR", "PROPILOT_HUD", 0), - ("FRONT_RADAR_ERROR", "PROPILOT_HUD", 0), - ("FRONT_RADAR_ERROR_FLASHING", "PROPILOT_HUD", 0), - ("SIDE_RADAR_ERROR_FLASHING3", "PROPILOT_HUD", 0), - ("LKAS_ERROR_FLASHING", "PROPILOT_HUD", 0), - ("SAFETY_SHIELD_ACTIVE", "PROPILOT_HUD", 0), - ("RIGHT_LANE_GREEN_FLASH", "PROPILOT_HUD", 0), - ("LEFT_LANE_GREEN_FLASH", "PROPILOT_HUD", 0), - ("FOLLOW_DISTANCE", "PROPILOT_HUD", 0), - ("AUDIBLE_TONE", "PROPILOT_HUD", 0), - ("SPEED_SET_ICON", "PROPILOT_HUD", 0), - ("SMALL_STEERING_WHEEL_ICON", "PROPILOT_HUD", 0), - ("unknown59", "PROPILOT_HUD", 0), - ("unknown55", "PROPILOT_HUD", 0), - ("unknown26", "PROPILOT_HUD", 0), - ("unknown28", "PROPILOT_HUD", 0), - ("unknown31", "PROPILOT_HUD", 0), - ("SET_SPEED", "PROPILOT_HUD", 0), - ("unknown43", "PROPILOT_HUD", 0), - ("unknown08", "PROPILOT_HUD", 0), - ("unknown05", "PROPILOT_HUD", 0), - ("unknown02", "PROPILOT_HUD", 0), - - ("NA_HIGH_ACCEL_TEMP", "PROPILOT_HUD_INFO_MSG", 0), - ("SIDE_RADAR_NA_HIGH_CABIN_TEMP", "PROPILOT_HUD_INFO_MSG", 0), - ("SIDE_RADAR_MALFUNCTION", "PROPILOT_HUD_INFO_MSG", 0), - ("LKAS_MALFUNCTION", "PROPILOT_HUD_INFO_MSG", 0), - ("FRONT_RADAR_MALFUNCTION", "PROPILOT_HUD_INFO_MSG", 0), - ("SIDE_RADAR_NA_CLEAN_REAR_CAMERA", "PROPILOT_HUD_INFO_MSG", 0), - ("NA_POOR_ROAD_CONDITIONS", "PROPILOT_HUD_INFO_MSG", 0), - ("CURRENTLY_UNAVAILABLE", "PROPILOT_HUD_INFO_MSG", 0), - ("SAFETY_SHIELD_OFF", "PROPILOT_HUD_INFO_MSG", 0), - ("FRONT_COLLISION_NA_FRONT_RADAR_OBSTRUCTION", "PROPILOT_HUD_INFO_MSG", 0), - ("PEDAL_MISSAPPLICATION_SYSTEM_ACTIVATED", "PROPILOT_HUD_INFO_MSG", 0), - ("SIDE_IMPACT_NA_RADAR_OBSTRUCTION", "PROPILOT_HUD_INFO_MSG", 0), - ("WARNING_DO_NOT_ENTER", "PROPILOT_HUD_INFO_MSG", 0), - ("SIDE_IMPACT_SYSTEM_OFF", "PROPILOT_HUD_INFO_MSG", 0), - ("SIDE_IMPACT_MALFUNCTION", "PROPILOT_HUD_INFO_MSG", 0), - ("FRONT_COLLISION_MALFUNCTION", "PROPILOT_HUD_INFO_MSG", 0), - ("SIDE_RADAR_MALFUNCTION2", "PROPILOT_HUD_INFO_MSG", 0), - ("LKAS_MALFUNCTION2", "PROPILOT_HUD_INFO_MSG", 0), - ("FRONT_RADAR_MALFUNCTION2", "PROPILOT_HUD_INFO_MSG", 0), - ("PROPILOT_NA_MSGS", "PROPILOT_HUD_INFO_MSG", 0), - ("BOTTOM_MSG", "PROPILOT_HUD_INFO_MSG", 0), - ("HANDS_ON_WHEEL_WARNING", "PROPILOT_HUD_INFO_MSG", 0), - ("WARNING_STEP_ON_BRAKE_NOW", "PROPILOT_HUD_INFO_MSG", 0), - ("PROPILOT_NA_FRONT_CAMERA_OBSTRUCTED", "PROPILOT_HUD_INFO_MSG", 0), - ("PROPILOT_NA_HIGH_CABIN_TEMP", "PROPILOT_HUD_INFO_MSG", 0), - ("WARNING_PROPILOT_MALFUNCTION", "PROPILOT_HUD_INFO_MSG", 0), - ("ACC_UNAVAILABLE_HIGH_CABIN_TEMP", "PROPILOT_HUD_INFO_MSG", 0), - ("ACC_NA_FRONT_CAMERA_IMPARED", "PROPILOT_HUD_INFO_MSG", 0), - ("unknown07", "PROPILOT_HUD_INFO_MSG", 0), - ("unknown10", "PROPILOT_HUD_INFO_MSG", 0), - ("unknown15", "PROPILOT_HUD_INFO_MSG", 0), - ("unknown23", "PROPILOT_HUD_INFO_MSG", 0), - ("unknown19", "PROPILOT_HUD_INFO_MSG", 0), - ("unknown31", "PROPILOT_HUD_INFO_MSG", 0), - ("unknown32", "PROPILOT_HUD_INFO_MSG", 0), - ("unknown46", "PROPILOT_HUD_INFO_MSG", 0), - ("unknown61", "PROPILOT_HUD_INFO_MSG", 0), - ("unknown55", "PROPILOT_HUD_INFO_MSG", 0), - ("unknown50", "PROPILOT_HUD_INFO_MSG", 0), + ("LARGE_WARNING_FLASHING", "PROPILOT_HUD"), + ("SIDE_RADAR_ERROR_FLASHING1", "PROPILOT_HUD"), + ("SIDE_RADAR_ERROR_FLASHING2", "PROPILOT_HUD"), + ("LEAD_CAR", "PROPILOT_HUD"), + ("LEAD_CAR_ERROR", "PROPILOT_HUD"), + ("FRONT_RADAR_ERROR", "PROPILOT_HUD"), + ("FRONT_RADAR_ERROR_FLASHING", "PROPILOT_HUD"), + ("SIDE_RADAR_ERROR_FLASHING3", "PROPILOT_HUD"), + ("LKAS_ERROR_FLASHING", "PROPILOT_HUD"), + ("SAFETY_SHIELD_ACTIVE", "PROPILOT_HUD"), + ("RIGHT_LANE_GREEN_FLASH", "PROPILOT_HUD"), + ("LEFT_LANE_GREEN_FLASH", "PROPILOT_HUD"), + ("FOLLOW_DISTANCE", "PROPILOT_HUD"), + ("AUDIBLE_TONE", "PROPILOT_HUD"), + ("SPEED_SET_ICON", "PROPILOT_HUD"), + ("SMALL_STEERING_WHEEL_ICON", "PROPILOT_HUD"), + ("unknown59", "PROPILOT_HUD"), + ("unknown55", "PROPILOT_HUD"), + ("unknown26", "PROPILOT_HUD"), + ("unknown28", "PROPILOT_HUD"), + ("unknown31", "PROPILOT_HUD"), + ("SET_SPEED", "PROPILOT_HUD"), + ("unknown43", "PROPILOT_HUD"), + ("unknown08", "PROPILOT_HUD"), + ("unknown05", "PROPILOT_HUD"), + ("unknown02", "PROPILOT_HUD"), + + ("NA_HIGH_ACCEL_TEMP", "PROPILOT_HUD_INFO_MSG"), + ("SIDE_RADAR_NA_HIGH_CABIN_TEMP", "PROPILOT_HUD_INFO_MSG"), + ("SIDE_RADAR_MALFUNCTION", "PROPILOT_HUD_INFO_MSG"), + ("LKAS_MALFUNCTION", "PROPILOT_HUD_INFO_MSG"), + ("FRONT_RADAR_MALFUNCTION", "PROPILOT_HUD_INFO_MSG"), + ("SIDE_RADAR_NA_CLEAN_REAR_CAMERA", "PROPILOT_HUD_INFO_MSG"), + ("NA_POOR_ROAD_CONDITIONS", "PROPILOT_HUD_INFO_MSG"), + ("CURRENTLY_UNAVAILABLE", "PROPILOT_HUD_INFO_MSG"), + ("SAFETY_SHIELD_OFF", "PROPILOT_HUD_INFO_MSG"), + ("FRONT_COLLISION_NA_FRONT_RADAR_OBSTRUCTION", "PROPILOT_HUD_INFO_MSG"), + ("PEDAL_MISSAPPLICATION_SYSTEM_ACTIVATED", "PROPILOT_HUD_INFO_MSG"), + ("SIDE_IMPACT_NA_RADAR_OBSTRUCTION", "PROPILOT_HUD_INFO_MSG"), + ("WARNING_DO_NOT_ENTER", "PROPILOT_HUD_INFO_MSG"), + ("SIDE_IMPACT_SYSTEM_OFF", "PROPILOT_HUD_INFO_MSG"), + ("SIDE_IMPACT_MALFUNCTION", "PROPILOT_HUD_INFO_MSG"), + ("FRONT_COLLISION_MALFUNCTION", "PROPILOT_HUD_INFO_MSG"), + ("SIDE_RADAR_MALFUNCTION2", "PROPILOT_HUD_INFO_MSG"), + ("LKAS_MALFUNCTION2", "PROPILOT_HUD_INFO_MSG"), + ("FRONT_RADAR_MALFUNCTION2", "PROPILOT_HUD_INFO_MSG"), + ("PROPILOT_NA_MSGS", "PROPILOT_HUD_INFO_MSG"), + ("BOTTOM_MSG", "PROPILOT_HUD_INFO_MSG"), + ("HANDS_ON_WHEEL_WARNING", "PROPILOT_HUD_INFO_MSG"), + ("WARNING_STEP_ON_BRAKE_NOW", "PROPILOT_HUD_INFO_MSG"), + ("PROPILOT_NA_FRONT_CAMERA_OBSTRUCTED", "PROPILOT_HUD_INFO_MSG"), + ("PROPILOT_NA_HIGH_CABIN_TEMP", "PROPILOT_HUD_INFO_MSG"), + ("WARNING_PROPILOT_MALFUNCTION", "PROPILOT_HUD_INFO_MSG"), + ("ACC_UNAVAILABLE_HIGH_CABIN_TEMP", "PROPILOT_HUD_INFO_MSG"), + ("ACC_NA_FRONT_CAMERA_IMPARED", "PROPILOT_HUD_INFO_MSG"), + ("unknown07", "PROPILOT_HUD_INFO_MSG"), + ("unknown10", "PROPILOT_HUD_INFO_MSG"), + ("unknown15", "PROPILOT_HUD_INFO_MSG"), + ("unknown23", "PROPILOT_HUD_INFO_MSG"), + ("unknown19", "PROPILOT_HUD_INFO_MSG"), + ("unknown31", "PROPILOT_HUD_INFO_MSG"), + ("unknown32", "PROPILOT_HUD_INFO_MSG"), + ("unknown46", "PROPILOT_HUD_INFO_MSG"), + ("unknown61", "PROPILOT_HUD_INFO_MSG"), + ("unknown55", "PROPILOT_HUD_INFO_MSG"), + ("unknown50", "PROPILOT_HUD_INFO_MSG"), ] checks = [ @@ -344,21 +339,12 @@ def get_cam_can_parser(CP): signals = [] checks = [] - if CP.carFingerprint in [CAR.ROGUE, CAR.XTRAIL]: - signals += [ - ("CRUISE_ON", "PRO_PILOT", 0), - ] - checks += [ - ("PRO_PILOT", 100), - ] + if CP.carFingerprint in (CAR.ROGUE, CAR.XTRAIL): + signals.append(("CRUISE_ON", "PRO_PILOT")) + checks.append(("PRO_PILOT", 100)) elif CP.carFingerprint == CAR.ALTIMA: - signals += [ - ("STEER_TORQUE_DRIVER", "STEER_TORQUE_SENSOR", 0), - ] - checks += [ - ("STEER_TORQUE_SENSOR", 100), - ] + signals.append(("STEER_TORQUE_DRIVER", "STEER_TORQUE_SENSOR")) + checks.append(("STEER_TORQUE_SENSOR", 100)) return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 0) - return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 1) diff --git a/selfdrive/car/nissan/interface.py b/selfdrive/car/nissan/interface.py index 86c1630fb641cb..c32fb13780fd7d 100644 --- a/selfdrive/car/nissan/interface.py +++ b/selfdrive/car/nissan/interface.py @@ -16,17 +16,17 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None): ret.carName = "nissan" ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.nissan)] - ret.steerLimitAlert = False + ret.steerLimitTimer = 1.0 ret.steerRateCost = 0.5 ret.steerActuatorDelay = 0.1 - if candidate in [CAR.ROGUE, CAR.XTRAIL]: + if candidate in (CAR.ROGUE, CAR.XTRAIL): ret.mass = 1610 + STD_CARGO_KG ret.wheelbase = 2.705 ret.centerToFront = ret.wheelbase * 0.44 ret.steerRatio = 17 - elif candidate in [CAR.LEAF, CAR.LEAF_IC]: + elif candidate in (CAR.LEAF, CAR.LEAF_IC): ret.mass = 1610 + STD_CARGO_KG ret.wheelbase = 2.705 ret.centerToFront = ret.wheelbase * 0.44 @@ -79,7 +79,7 @@ def update(self, c, can_strings): def apply(self, c): hud_control = c.hudControl - ret = self.CC.update(c.enabled, self.CS, self.frame, c.actuators, + ret = self.CC.update(c, c.enabled, self.CS, self.frame, c.actuators, c.cruiseControl.cancel, hud_control.visualAlert, hud_control.leftLaneVisible, hud_control.rightLaneVisible, hud_control.leftLaneDepart, hud_control.rightLaneDepart) diff --git a/selfdrive/car/nissan/values.py b/selfdrive/car/nissan/values.py index 22bd4a1295f094..f7001d1417417c 100644 --- a/selfdrive/car/nissan/values.py +++ b/selfdrive/car/nissan/values.py @@ -1,5 +1,3 @@ -# flake8: noqa - from selfdrive.car import dbc_dict from cereal import car Ecu = car.CarParams.Ecu @@ -71,7 +69,7 @@ class CAR: (Ecu.gateway, 0x18dad0f1, None): [ b'284U29HE0A', ], - }, + }, CAR.LEAF_IC: { (Ecu.fwdCamera, 0x707, None): [ b'5SH1BDB\x04\x18\x00\x00\x00\x00\x00_-?\x04\x91\xf2\x00\x00\x00\x80', diff --git a/selfdrive/car/subaru/carcontroller.py b/selfdrive/car/subaru/carcontroller.py index 72b07f919261bd..afc91f4755b584 100644 --- a/selfdrive/car/subaru/carcontroller.py +++ b/selfdrive/car/subaru/carcontroller.py @@ -15,7 +15,7 @@ def __init__(self, dbc_name, CP, VM): self.p = CarControllerParams(CP) self.packer = CANPacker(DBC[CP.carFingerprint]['pt']) - def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, visual_alert, left_line, right_line, left_lane_depart, right_lane_depart): + def update(self, c, enabled, CS, frame, actuators, pcm_cancel_cmd, visual_alert, left_line, right_line, left_lane_depart, right_lane_depart): can_sends = [] @@ -30,7 +30,7 @@ def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, visual_alert, le apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, self.p) self.steer_rate_limited = new_steer != apply_steer - if not enabled: + if not c.active: apply_steer = 0 if CS.CP.carFingerprint in PREGLOBAL_CARS: diff --git a/selfdrive/car/subaru/carstate.py b/selfdrive/car/subaru/carstate.py index e9728e557b5351..1eefb18584cd26 100644 --- a/selfdrive/car/subaru/carstate.py +++ b/selfdrive/car/subaru/carstate.py @@ -77,28 +77,27 @@ def update(self, cp, cp_cam): @staticmethod def get_can_parser(CP): - # this function generates lists for signal, messages and initial values signals = [ - # sig_name, sig_address, default - ("Steer_Torque_Sensor", "Steering_Torque", 0), - ("Steering_Angle", "Steering_Torque", 0), - ("Steer_Error_1", "Steering_Torque", 0), - ("Cruise_On", "CruiseControl", 0), - ("Cruise_Activated", "CruiseControl", 0), - ("Brake_Pedal", "Brake_Pedal", 0), - ("Throttle_Pedal", "Throttle", 0), - ("LEFT_BLINKER", "Dashlights", 0), - ("RIGHT_BLINKER", "Dashlights", 0), - ("SEATBELT_FL", "Dashlights", 0), - ("FL", "Wheel_Speeds", 0), - ("FR", "Wheel_Speeds", 0), - ("RL", "Wheel_Speeds", 0), - ("RR", "Wheel_Speeds", 0), - ("DOOR_OPEN_FR", "BodyInfo", 1), - ("DOOR_OPEN_FL", "BodyInfo", 1), - ("DOOR_OPEN_RR", "BodyInfo", 1), - ("DOOR_OPEN_RL", "BodyInfo", 1), - ("Gear", "Transmission", 0), + # sig_name, sig_address + ("Steer_Torque_Sensor", "Steering_Torque"), + ("Steering_Angle", "Steering_Torque"), + ("Steer_Error_1", "Steering_Torque"), + ("Cruise_On", "CruiseControl"), + ("Cruise_Activated", "CruiseControl"), + ("Brake_Pedal", "Brake_Pedal"), + ("Throttle_Pedal", "Throttle"), + ("LEFT_BLINKER", "Dashlights"), + ("RIGHT_BLINKER", "Dashlights"), + ("SEATBELT_FL", "Dashlights"), + ("FL", "Wheel_Speeds"), + ("FR", "Wheel_Speeds"), + ("RL", "Wheel_Speeds"), + ("RR", "Wheel_Speeds"), + ("DOOR_OPEN_FR", "BodyInfo"), + ("DOOR_OPEN_FL", "BodyInfo"), + ("DOOR_OPEN_RR", "BodyInfo"), + ("DOOR_OPEN_RL", "BodyInfo"), + ("Gear", "Transmission"), ] checks = [ @@ -114,20 +113,18 @@ def get_can_parser(CP): if CP.enableBsm: signals += [ - ("L_ADJACENT", "BSD_RCTA", 0), - ("R_ADJACENT", "BSD_RCTA", 0), - ("L_APPROACHING", "BSD_RCTA", 0), - ("R_APPROACHING", "BSD_RCTA", 0), - ] - checks += [ - ("BSD_RCTA", 17), + ("L_ADJACENT", "BSD_RCTA"), + ("R_ADJACENT", "BSD_RCTA"), + ("L_APPROACHING", "BSD_RCTA"), + ("R_APPROACHING", "BSD_RCTA"), ] + checks.append(("BSD_RCTA", 17)) if CP.carFingerprint not in PREGLOBAL_CARS: signals += [ - ("Steer_Warning", "Steering_Torque", 0), - ("Brake", "Brake_Status", 0), - ("UNITS", "Dashlights", 0), + ("Steer_Warning", "Steering_Torque"), + ("Brake", "Brake_Status"), + ("UNITS", "Dashlights"), ] checks += [ @@ -137,13 +134,9 @@ def get_can_parser(CP): ("CruiseControl", 20), ] else: - signals += [ - ("UNITS", "Dash_State2", 0), - ] + signals.append(("UNITS", "Dash_State2")) - checks += [ - ("Dash_State2", 1), - ] + checks.append(("Dash_State2", 1)) if CP.carFingerprint == CAR.FORESTER_PREGLOBAL: checks += [ @@ -152,7 +145,7 @@ def get_can_parser(CP): ("CruiseControl", 50), ] - if CP.carFingerprint in [CAR.LEGACY_PREGLOBAL, CAR.OUTBACK_PREGLOBAL, CAR.OUTBACK_PREGLOBAL_2018]: + if CP.carFingerprint in (CAR.LEGACY_PREGLOBAL, CAR.OUTBACK_PREGLOBAL, CAR.OUTBACK_PREGLOBAL_2018): checks += [ ("Dashlights", 10), ("CruiseControl", 50), @@ -164,26 +157,26 @@ def get_can_parser(CP): def get_cam_can_parser(CP): if CP.carFingerprint in PREGLOBAL_CARS: signals = [ - ("Cruise_Set_Speed", "ES_DashStatus", 0), - ("Not_Ready_Startup", "ES_DashStatus", 0), - - ("Cruise_Throttle", "ES_Distance", 0), - ("Signal1", "ES_Distance", 0), - ("Car_Follow", "ES_Distance", 0), - ("Signal2", "ES_Distance", 0), - ("Brake_On", "ES_Distance", 0), - ("Distance_Swap", "ES_Distance", 0), - ("Standstill", "ES_Distance", 0), - ("Signal3", "ES_Distance", 0), - ("Close_Distance", "ES_Distance", 0), - ("Signal4", "ES_Distance", 0), - ("Standstill_2", "ES_Distance", 0), - ("Cruise_Fault", "ES_Distance", 0), - ("Signal5", "ES_Distance", 0), - ("Counter", "ES_Distance", 0), - ("Signal6", "ES_Distance", 0), - ("Cruise_Button", "ES_Distance", 0), - ("Signal7", "ES_Distance", 0), + ("Cruise_Set_Speed", "ES_DashStatus"), + ("Not_Ready_Startup", "ES_DashStatus"), + + ("Cruise_Throttle", "ES_Distance"), + ("Signal1", "ES_Distance"), + ("Car_Follow", "ES_Distance"), + ("Signal2", "ES_Distance"), + ("Brake_On", "ES_Distance"), + ("Distance_Swap", "ES_Distance"), + ("Standstill", "ES_Distance"), + ("Signal3", "ES_Distance"), + ("Close_Distance", "ES_Distance"), + ("Signal4", "ES_Distance"), + ("Standstill_2", "ES_Distance"), + ("Cruise_Fault", "ES_Distance"), + ("Signal5", "ES_Distance"), + ("Counter", "ES_Distance"), + ("Signal6", "ES_Distance"), + ("Cruise_Button", "ES_Distance"), + ("Signal7", "ES_Distance"), ] checks = [ @@ -192,42 +185,42 @@ def get_cam_can_parser(CP): ] else: signals = [ - ("Cruise_Set_Speed", "ES_DashStatus", 0), - ("Conventional_Cruise", "ES_DashStatus", 0), - - ("Counter", "ES_Distance", 0), - ("Signal1", "ES_Distance", 0), - ("Cruise_Fault", "ES_Distance", 0), - ("Cruise_Throttle", "ES_Distance", 0), - ("Signal2", "ES_Distance", 0), - ("Car_Follow", "ES_Distance", 0), - ("Signal3", "ES_Distance", 0), - ("Cruise_Brake_Active", "ES_Distance", 0), - ("Distance_Swap", "ES_Distance", 0), - ("Cruise_EPB", "ES_Distance", 0), - ("Signal4", "ES_Distance", 0), - ("Close_Distance", "ES_Distance", 0), - ("Signal5", "ES_Distance", 0), - ("Cruise_Cancel", "ES_Distance", 0), - ("Cruise_Set", "ES_Distance", 0), - ("Cruise_Resume", "ES_Distance", 0), - ("Signal6", "ES_Distance", 0), - - ("Counter", "ES_LKAS_State", 0), - ("LKAS_Alert_Msg", "ES_LKAS_State", 0), - ("Signal1", "ES_LKAS_State", 0), - ("LKAS_ACTIVE", "ES_LKAS_State", 0), - ("LKAS_Dash_State", "ES_LKAS_State", 0), - ("Signal2", "ES_LKAS_State", 0), - ("Backward_Speed_Limit_Menu", "ES_LKAS_State", 0), - ("LKAS_Left_Line_Enable", "ES_LKAS_State", 0), - ("LKAS_Left_Line_Light_Blink", "ES_LKAS_State", 0), - ("LKAS_Right_Line_Enable", "ES_LKAS_State", 0), - ("LKAS_Right_Line_Light_Blink", "ES_LKAS_State", 0), - ("LKAS_Left_Line_Visible", "ES_LKAS_State", 0), - ("LKAS_Right_Line_Visible", "ES_LKAS_State", 0), - ("LKAS_Alert", "ES_LKAS_State", 0), - ("Signal3", "ES_LKAS_State", 0), + ("Cruise_Set_Speed", "ES_DashStatus"), + ("Conventional_Cruise", "ES_DashStatus"), + + ("Counter", "ES_Distance"), + ("Signal1", "ES_Distance"), + ("Cruise_Fault", "ES_Distance"), + ("Cruise_Throttle", "ES_Distance"), + ("Signal2", "ES_Distance"), + ("Car_Follow", "ES_Distance"), + ("Signal3", "ES_Distance"), + ("Cruise_Brake_Active", "ES_Distance"), + ("Distance_Swap", "ES_Distance"), + ("Cruise_EPB", "ES_Distance"), + ("Signal4", "ES_Distance"), + ("Close_Distance", "ES_Distance"), + ("Signal5", "ES_Distance"), + ("Cruise_Cancel", "ES_Distance"), + ("Cruise_Set", "ES_Distance"), + ("Cruise_Resume", "ES_Distance"), + ("Signal6", "ES_Distance"), + + ("Counter", "ES_LKAS_State"), + ("LKAS_Alert_Msg", "ES_LKAS_State"), + ("Signal1", "ES_LKAS_State"), + ("LKAS_ACTIVE", "ES_LKAS_State"), + ("LKAS_Dash_State", "ES_LKAS_State"), + ("Signal2", "ES_LKAS_State"), + ("Backward_Speed_Limit_Menu", "ES_LKAS_State"), + ("LKAS_Left_Line_Enable", "ES_LKAS_State"), + ("LKAS_Left_Line_Light_Blink", "ES_LKAS_State"), + ("LKAS_Right_Line_Enable", "ES_LKAS_State"), + ("LKAS_Right_Line_Light_Blink", "ES_LKAS_State"), + ("LKAS_Left_Line_Visible", "ES_LKAS_State"), + ("LKAS_Right_Line_Visible", "ES_LKAS_State"), + ("LKAS_Alert", "ES_LKAS_State"), + ("Signal3", "ES_LKAS_State"), ] checks = [ diff --git a/selfdrive/car/subaru/interface.py b/selfdrive/car/subaru/interface.py index 0fe0f1c401aa76..ae4655014858cd 100644 --- a/selfdrive/car/subaru/interface.py +++ b/selfdrive/car/subaru/interface.py @@ -66,7 +66,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None): ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0., 14., 23.], [0., 14., 23.]] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.01, 0.065, 0.2], [0.001, 0.015, 0.025]] - if candidate in [CAR.FORESTER_PREGLOBAL, CAR.OUTBACK_PREGLOBAL_2018]: + if candidate in (CAR.FORESTER_PREGLOBAL, CAR.OUTBACK_PREGLOBAL_2018): ret.safetyConfigs[0].safetyParam = 1 # Outback 2018-2019 and Forester have reversed driver torque signal ret.mass = 1568 + STD_CARGO_KG ret.wheelbase = 2.67 @@ -124,7 +124,7 @@ def update(self, c, can_strings): def apply(self, c): hud_control = c.hudControl - ret = self.CC.update(c.enabled, self.CS, self.frame, c.actuators, + ret = self.CC.update(c, c.enabled, self.CS, self.frame, c.actuators, c.cruiseControl.cancel, hud_control.visualAlert, hud_control.leftLaneVisible, hud_control.rightLaneVisible, hud_control.leftLaneDepart, hud_control.rightLaneDepart) self.frame += 1 diff --git a/selfdrive/car/subaru/values.py b/selfdrive/car/subaru/values.py index 5ec95b00763602..4e174536634f7b 100644 --- a/selfdrive/car/subaru/values.py +++ b/selfdrive/car/subaru/values.py @@ -1,5 +1,3 @@ -# flake8: noqa - from selfdrive.car import dbc_dict from cereal import car Ecu = car.CarParams.Ecu @@ -112,8 +110,7 @@ class CAR: b'\xaa!`u\a', b'\xaa!dq\a', b'\xaa!dt\a', - b'\xf1\x00\xa2\x10\t' - b'\xc5!dr\a', + b'\xf1\x00\xa2\x10\t', b'\xc5!ar\a', b'\xbe!as\a', b'\xc5!ds\a', @@ -131,7 +128,6 @@ class CAR: b'\xe3\xf5G\x00\x00', b'\xe3\xf5\a\x00\x00', b'\xe3\xf5C\x00\x00', - b'\xf1\x00\xa4\x10@' b'\xe5\xf5B\x00\x00', b'\xe5\xf5$\000\000', b'\xe4\xf5\a\000\000', @@ -197,7 +193,7 @@ class CAR: b'\032\xf6B0\000', b'\032\xf6F`\000', b'\032\xf6b`\000', - b'\032\xf6B`\000' + b'\032\xf6B`\000', b'\xf1\x00\xa4\x10@', ], }, @@ -298,7 +294,7 @@ class CAR: b'\xab"@s\a', b'\xab+@@\a', b'\xb4"@r\a', - b'\xa0+@@\x07' + b'\xa0+@@\x07', b'\xa0\"@\x80\a', ], (Ecu.transmission, 0x7e1, None): [ diff --git a/selfdrive/car/tesla/carcontroller.py b/selfdrive/car/tesla/carcontroller.py index 4efd1c1fe7c4bf..03f09f2407bd7a 100644 --- a/selfdrive/car/tesla/carcontroller.py +++ b/selfdrive/car/tesla/carcontroller.py @@ -12,12 +12,12 @@ def __init__(self, dbc_name, CP, VM): self.pt_packer = CANPacker(DBC[CP.carFingerprint]['pt']) self.tesla_can = TeslaCAN(self.packer, self.pt_packer) - def update(self, enabled, CS, frame, actuators, cruise_cancel): + def update(self, c, enabled, CS, frame, actuators, cruise_cancel): can_sends = [] # Temp disable steering on a hands_on_fault, and allow for user override hands_on_fault = (CS.steer_warning == "EAC_ERROR_HANDS_ON" and CS.hands_on_level >= 3) - lkas_enabled = enabled and (not hands_on_fault) + lkas_enabled = c.active and (not hands_on_fault) if lkas_enabled: apply_angle = actuators.steeringAngleDeg @@ -37,7 +37,7 @@ def update(self, enabled, CS, frame, actuators, cruise_cancel): can_sends.append(self.tesla_can.create_steering_control(apply_angle, lkas_enabled, frame)) # Longitudinal control (40Hz) - if self.CP.openpilotLongitudinalControl and ((frame % 5) in [0, 2]): + if self.CP.openpilotLongitudinalControl and ((frame % 5) in (0, 2)): target_accel = actuators.accel target_speed = max(CS.out.vEgo + (target_accel * CarControllerParams.ACCEL_TO_SPEED_MULTIPLIER), 0) max_accel = 0 if target_accel < 0 else target_accel @@ -65,4 +65,4 @@ def update(self, enabled, CS, frame, actuators, cruise_cancel): new_actuators = actuators.copy() new_actuators.steeringAngleDeg = apply_angle - return actuators, can_sends + return new_actuators, can_sends diff --git a/selfdrive/car/tesla/carstate.py b/selfdrive/car/tesla/carstate.py index 0a45b6f2bb3712..51ae43ad1bc184 100644 --- a/selfdrive/car/tesla/carstate.py +++ b/selfdrive/car/tesla/carstate.py @@ -50,7 +50,7 @@ def update(self, cp, cp_cam): cruise_state = self.can_define.dv["DI_state"]["DI_cruiseState"].get(int(cp.vl["DI_state"]["DI_cruiseState"]), None) speed_units = self.can_define.dv["DI_state"]["DI_speedUnits"].get(int(cp.vl["DI_state"]["DI_speedUnits"]), None) - acc_enabled = (cruise_state in ["ENABLED", "STANDSTILL", "OVERRIDE", "PRE_FAULT", "PRE_CANCEL"]) + acc_enabled = (cruise_state in ("ENABLED", "STANDSTILL", "OVERRIDE", "PRE_FAULT", "PRE_CANCEL")) ret.cruiseState.enabled = acc_enabled if speed_units == "KPH": @@ -96,64 +96,64 @@ def update(self, cp, cp_cam): @staticmethod def get_can_parser(CP): signals = [ - # sig_name, sig_address, default - ("ESP_vehicleSpeed", "ESP_B", 0), - ("DI_pedalPos", "DI_torque1", 0), - ("DI_brakePedal", "DI_torque2", 0), - ("StW_AnglHP", "STW_ANGLHP_STAT", 0), - ("StW_AnglHP_Spd", "STW_ANGLHP_STAT", 0), - ("EPAS_handsOnLevel", "EPAS_sysStatus", 0), - ("EPAS_torsionBarTorque", "EPAS_sysStatus", 0), - ("EPAS_internalSAS", "EPAS_sysStatus", 0), - ("EPAS_eacStatus", "EPAS_sysStatus", 1), - ("EPAS_eacErrorCode", "EPAS_sysStatus", 0), - ("DI_cruiseState", "DI_state", 0), - ("DI_digitalSpeed", "DI_state", 0), - ("DI_speedUnits", "DI_state", 0), - ("DI_gear", "DI_torque2", 0), - ("DOOR_STATE_FL", "GTW_carState", 1), - ("DOOR_STATE_FR", "GTW_carState", 1), - ("DOOR_STATE_RL", "GTW_carState", 1), - ("DOOR_STATE_RR", "GTW_carState", 1), - ("DOOR_STATE_FrontTrunk", "GTW_carState", 1), - ("BOOT_STATE", "GTW_carState", 1), - ("BC_indicatorLStatus", "GTW_carState", 1), - ("BC_indicatorRStatus", "GTW_carState", 1), - ("SDM_bcklDrivStatus", "SDM1", 0), - ("driverBrakeStatus", "BrakeMessage", 0), + # sig_name, sig_address + ("ESP_vehicleSpeed", "ESP_B"), + ("DI_pedalPos", "DI_torque1"), + ("DI_brakePedal", "DI_torque2"), + ("StW_AnglHP", "STW_ANGLHP_STAT"), + ("StW_AnglHP_Spd", "STW_ANGLHP_STAT"), + ("EPAS_handsOnLevel", "EPAS_sysStatus"), + ("EPAS_torsionBarTorque", "EPAS_sysStatus"), + ("EPAS_internalSAS", "EPAS_sysStatus"), + ("EPAS_eacStatus", "EPAS_sysStatus"), + ("EPAS_eacErrorCode", "EPAS_sysStatus"), + ("DI_cruiseState", "DI_state"), + ("DI_digitalSpeed", "DI_state"), + ("DI_speedUnits", "DI_state"), + ("DI_gear", "DI_torque2"), + ("DOOR_STATE_FL", "GTW_carState"), + ("DOOR_STATE_FR", "GTW_carState"), + ("DOOR_STATE_RL", "GTW_carState"), + ("DOOR_STATE_RR", "GTW_carState"), + ("DOOR_STATE_FrontTrunk", "GTW_carState"), + ("BOOT_STATE", "GTW_carState"), + ("BC_indicatorLStatus", "GTW_carState"), + ("BC_indicatorRStatus", "GTW_carState"), + ("SDM_bcklDrivStatus", "SDM1"), + ("driverBrakeStatus", "BrakeMessage"), # We copy this whole message when spamming cancel - ("SpdCtrlLvr_Stat", "STW_ACTN_RQ", 0), - ("VSL_Enbl_Rq", "STW_ACTN_RQ", 0), - ("SpdCtrlLvrStat_Inv", "STW_ACTN_RQ", 0), - ("DTR_Dist_Rq", "STW_ACTN_RQ", 0), - ("TurnIndLvr_Stat", "STW_ACTN_RQ", 0), - ("HiBmLvr_Stat", "STW_ACTN_RQ", 0), - ("WprWashSw_Psd", "STW_ACTN_RQ", 0), - ("WprWash_R_Sw_Posn_V2", "STW_ACTN_RQ", 0), - ("StW_Lvr_Stat", "STW_ACTN_RQ", 0), - ("StW_Cond_Flt", "STW_ACTN_RQ", 0), - ("StW_Cond_Psd", "STW_ACTN_RQ", 0), - ("HrnSw_Psd", "STW_ACTN_RQ", 0), - ("StW_Sw00_Psd", "STW_ACTN_RQ", 0), - ("StW_Sw01_Psd", "STW_ACTN_RQ", 0), - ("StW_Sw02_Psd", "STW_ACTN_RQ", 0), - ("StW_Sw03_Psd", "STW_ACTN_RQ", 0), - ("StW_Sw04_Psd", "STW_ACTN_RQ", 0), - ("StW_Sw05_Psd", "STW_ACTN_RQ", 0), - ("StW_Sw06_Psd", "STW_ACTN_RQ", 0), - ("StW_Sw07_Psd", "STW_ACTN_RQ", 0), - ("StW_Sw08_Psd", "STW_ACTN_RQ", 0), - ("StW_Sw09_Psd", "STW_ACTN_RQ", 0), - ("StW_Sw10_Psd", "STW_ACTN_RQ", 0), - ("StW_Sw11_Psd", "STW_ACTN_RQ", 0), - ("StW_Sw12_Psd", "STW_ACTN_RQ", 0), - ("StW_Sw13_Psd", "STW_ACTN_RQ", 0), - ("StW_Sw14_Psd", "STW_ACTN_RQ", 0), - ("StW_Sw15_Psd", "STW_ACTN_RQ", 0), - ("WprSw6Posn", "STW_ACTN_RQ", 0), - ("MC_STW_ACTN_RQ", "STW_ACTN_RQ", 0), - ("CRC_STW_ACTN_RQ", "STW_ACTN_RQ", 0), + ("SpdCtrlLvr_Stat", "STW_ACTN_RQ"), + ("VSL_Enbl_Rq", "STW_ACTN_RQ"), + ("SpdCtrlLvrStat_Inv", "STW_ACTN_RQ"), + ("DTR_Dist_Rq", "STW_ACTN_RQ"), + ("TurnIndLvr_Stat", "STW_ACTN_RQ"), + ("HiBmLvr_Stat", "STW_ACTN_RQ"), + ("WprWashSw_Psd", "STW_ACTN_RQ"), + ("WprWash_R_Sw_Posn_V2", "STW_ACTN_RQ"), + ("StW_Lvr_Stat", "STW_ACTN_RQ"), + ("StW_Cond_Flt", "STW_ACTN_RQ"), + ("StW_Cond_Psd", "STW_ACTN_RQ"), + ("HrnSw_Psd", "STW_ACTN_RQ"), + ("StW_Sw00_Psd", "STW_ACTN_RQ"), + ("StW_Sw01_Psd", "STW_ACTN_RQ"), + ("StW_Sw02_Psd", "STW_ACTN_RQ"), + ("StW_Sw03_Psd", "STW_ACTN_RQ"), + ("StW_Sw04_Psd", "STW_ACTN_RQ"), + ("StW_Sw05_Psd", "STW_ACTN_RQ"), + ("StW_Sw06_Psd", "STW_ACTN_RQ"), + ("StW_Sw07_Psd", "STW_ACTN_RQ"), + ("StW_Sw08_Psd", "STW_ACTN_RQ"), + ("StW_Sw09_Psd", "STW_ACTN_RQ"), + ("StW_Sw10_Psd", "STW_ACTN_RQ"), + ("StW_Sw11_Psd", "STW_ACTN_RQ"), + ("StW_Sw12_Psd", "STW_ACTN_RQ"), + ("StW_Sw13_Psd", "STW_ACTN_RQ"), + ("StW_Sw14_Psd", "STW_ACTN_RQ"), + ("StW_Sw15_Psd", "STW_ACTN_RQ"), + ("WprSw6Posn", "STW_ACTN_RQ"), + ("MC_STW_ACTN_RQ", "STW_ACTN_RQ"), + ("CRC_STW_ACTN_RQ", "STW_ACTN_RQ"), ] checks = [ @@ -175,8 +175,8 @@ def get_can_parser(CP): @staticmethod def get_cam_can_parser(CP): signals = [ - # sig_name, sig_address, default - ("DAS_accState", "DAS_control", 0), + # sig_name, sig_address + ("DAS_accState", "DAS_control"), ] checks = [ # sig_address, frequency diff --git a/selfdrive/car/tesla/interface.py b/selfdrive/car/tesla/interface.py index 0a5067d1ce04e3..03012bc52eb312 100755 --- a/selfdrive/car/tesla/interface.py +++ b/selfdrive/car/tesla/interface.py @@ -40,10 +40,11 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None): ret.openpilotLongitudinalControl = False ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.tesla, 0)] + ret.steerLimitTimer = 1.0 ret.steerActuatorDelay = 0.1 ret.steerRateCost = 0.5 - if candidate in [CAR.AP2_MODELS, CAR.AP1_MODELS]: + if candidate in (CAR.AP2_MODELS, CAR.AP1_MODELS): ret.mass = 2100. + STD_CARGO_KG ret.wheelbase = 2.959 ret.centerToFront = ret.wheelbase * 0.5 @@ -70,6 +71,6 @@ def update(self, c, can_strings): return self.CS.out def apply(self, c): - ret = self.CC.update(c.enabled, self.CS, self.frame, c.actuators, c.cruiseControl.cancel) + ret = self.CC.update(c, c.enabled, self.CS, self.frame, c.actuators, c.cruiseControl.cancel) self.frame += 1 return ret diff --git a/selfdrive/car/tesla/radar_interface.py b/selfdrive/car/tesla/radar_interface.py index f5ad12ba7e89df..a09f53e75824e7 100755 --- a/selfdrive/car/tesla/radar_interface.py +++ b/selfdrive/car/tesla/radar_interface.py @@ -11,9 +11,9 @@ def get_radar_can_parser(CP): # Status messages signals = [ - ('RADC_HWFail', 'TeslaRadarSguInfo', 0), - ('RADC_SGUFail', 'TeslaRadarSguInfo', 0), - ('RADC_SensorDirty', 'TeslaRadarSguInfo', 0), + ('RADC_HWFail', 'TeslaRadarSguInfo'), + ('RADC_SGUFail', 'TeslaRadarSguInfo'), + ('RADC_SensorDirty', 'TeslaRadarSguInfo'), ] checks = [ @@ -29,16 +29,16 @@ def get_radar_can_parser(CP): # There is a bunch more info in the messages, # but these are the only things actually used in openpilot signals.extend([ - ('LongDist', msg_id_a, 255), - ('LongSpeed', msg_id_a, 0), - ('LatDist', msg_id_a, 0), - ('LongAccel', msg_id_a, 0), - ('Meas', msg_id_a, 0), - ('Tracked', msg_id_a, 0), - ('Index', msg_id_a, 0), - - ('LatSpeed', msg_id_b, 0), - ('Index2', msg_id_b, 0), + ('LongDist', msg_id_a), + ('LongSpeed', msg_id_a), + ('LatDist', msg_id_a), + ('LongAccel', msg_id_a), + ('Meas', msg_id_a), + ('Tracked', msg_id_a), + ('Index', msg_id_a), + + ('LatSpeed', msg_id_b), + ('Index2', msg_id_b), ]) checks.extend([ diff --git a/selfdrive/car/tesla/values.py b/selfdrive/car/tesla/values.py index 90bc45c7a578db..616933789e7134 100644 --- a/selfdrive/car/tesla/values.py +++ b/selfdrive/car/tesla/values.py @@ -1,5 +1,3 @@ -# flake8: noqa - from collections import namedtuple from selfdrive.car import dbc_dict from cereal import car diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index 07c4707b6186ed..efa4fa03500e2c 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -29,12 +29,12 @@ def update(self, enabled, active, CS, frame, actuators, pcm_cancel_cmd, hud_aler left_line, right_line, lead, left_lane_depart, right_lane_depart): # gas and brake - if CS.CP.enableGasInterceptor and enabled: + if CS.CP.enableGasInterceptor and active: MAX_INTERCEPTOR_GAS = 0.5 # RAV4 has very sensitive gas pedal - if CS.CP.carFingerprint in [CAR.RAV4, CAR.RAV4H, CAR.HIGHLANDER, CAR.HIGHLANDERH]: + if CS.CP.carFingerprint in (CAR.RAV4, CAR.RAV4H, CAR.HIGHLANDER, CAR.HIGHLANDERH): PEDAL_SCALE = interp(CS.out.vEgo, [0.0, MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_TRANSITION], [0.15, 0.3, 0.0]) - elif CS.CP.carFingerprint in [CAR.COROLLA]: + elif CS.CP.carFingerprint in (CAR.COROLLA,): PEDAL_SCALE = interp(CS.out.vEgo, [0.0, MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_TRANSITION], [0.3, 0.4, 0.0]) else: PEDAL_SCALE = interp(CS.out.vEgo, [0.0, MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_TRANSITION], [0.4, 0.5, 0.0]) @@ -52,7 +52,7 @@ def update(self, enabled, active, CS, frame, actuators, pcm_cancel_cmd, hud_aler self.steer_rate_limited = new_steer != apply_steer # Cut steering while we're in a known fault state (2s) - if not enabled or CS.steer_state in [9, 25] or abs(CS.out.steeringRateDeg) > 100: + if not active or CS.steer_state in (9, 25) or abs(CS.out.steeringRateDeg) > 100: apply_steer = 0 apply_steer_req = 0 else: @@ -95,7 +95,7 @@ def update(self, enabled, active, CS, frame, actuators, pcm_cancel_cmd, hud_aler lead = lead or CS.out.vEgo < 12. # at low speed we always assume the lead is present so ACC can be engaged # Lexus IS uses a different cancellation message - if pcm_cancel_cmd and CS.CP.carFingerprint in [CAR.LEXUS_IS, CAR.LEXUS_RC]: + if pcm_cancel_cmd and CS.CP.carFingerprint in (CAR.LEXUS_IS, CAR.LEXUS_RC): can_sends.append(create_acc_cancel_command(self.packer)) elif CS.CP.openpilotLongitudinalControl: can_sends.append(create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.standstill_req, lead, CS.acc_type, CS.distance_btn)) @@ -109,11 +109,11 @@ def update(self, enabled, active, CS, frame, actuators, pcm_cancel_cmd, hud_aler can_sends.append(create_gas_interceptor_command(self.packer, interceptor_gas_cmd, frame // 2)) self.gas = interceptor_gas_cmd - # ui mesg is at 100Hz but we send asap if: + # ui mesg is at 1Hz but we send asap if: # - there is something to display # - there is something to stop displaying fcw_alert = hud_alert == VisualAlert.fcw - steer_alert = hud_alert in [VisualAlert.steerRequired, VisualAlert.ldw] + steer_alert = hud_alert in (VisualAlert.steerRequired, VisualAlert.ldw) send_ui = False if ((fcw_alert or steer_alert) and not self.alert_active) or \ diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py index 743546c5debd08..39202c40f16969 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/selfdrive/car/toyota/carstate.py @@ -8,7 +8,7 @@ from selfdrive.car.interfaces import CarStateBase from opendbc.can.parser import CANParser from selfdrive.config import Conversions as CV -from selfdrive.car.toyota.values import CAR, DBC, STEER_THRESHOLD, NO_STOP_TIMER_CAR, TSS2_CAR +from selfdrive.car.toyota.values import ToyotaFlags, CAR, DBC, STEER_THRESHOLD, NO_STOP_TIMER_CAR, TSS2_CAR, EPS_SCALE class CarState(CarStateBase): @@ -16,6 +16,7 @@ def __init__(self, CP): super().__init__(CP) can_define = CANDefine(DBC[CP.carFingerprint]["pt"]) self.shifter_values = can_define.dv["GEAR_PACKET"]["GEAR"] + self.eps_torque_scale = EPS_SCALE[CP.carFingerprint] / 100. # On cars with cp.vl["STEER_TORQUE_SENSOR"]["STEER_ANGLE"] # the signal is zeroed to where the steering angle is at start. @@ -40,9 +41,9 @@ def __init__(self, CP): def update(self, cp, cp_cam): ret = car.CarState.new_message() - ret.doorOpen = any([cp.vl["SEATS_DOORS"]["DOOR_OPEN_FL"], cp.vl["SEATS_DOORS"]["DOOR_OPEN_FR"], - cp.vl["SEATS_DOORS"]["DOOR_OPEN_RL"], cp.vl["SEATS_DOORS"]["DOOR_OPEN_RR"]]) - ret.seatbeltUnlatched = cp.vl["SEATS_DOORS"]["SEATBELT_DRIVER_UNLATCHED"] != 0 + ret.doorOpen = any([cp.vl["BODY_CONTROL_STATE"]["DOOR_OPEN_FL"], cp.vl["BODY_CONTROL_STATE"]["DOOR_OPEN_FR"], + cp.vl["BODY_CONTROL_STATE"]["DOOR_OPEN_RL"], cp.vl["BODY_CONTROL_STATE"]["DOOR_OPEN_RR"]]) + ret.seatbeltUnlatched = cp.vl["BODY_CONTROL_STATE"]["SEATBELT_DRIVER_UNLATCHED"] != 0 ret.brakePressed = cp.vl["BRAKE_MODULE"]["BRAKE_PRESSED"] != 0 ret.brakeHoldActive = cp.vl["ESP_CONTROL"]["BRAKE_HOLD_ACTIVE"] == 1 @@ -50,7 +51,9 @@ def update(self, cp, cp_cam): ret.gas = (cp.vl["GAS_SENSOR"]["INTERCEPTOR_GAS"] + cp.vl["GAS_SENSOR"]["INTERCEPTOR_GAS2"]) / 2. ret.gasPressed = ret.gas > 15 else: - ret.gas = cp.vl["GAS_PEDAL"]["GAS_PEDAL"] + # TODO: find a new, common signal + msg = "GAS_PEDAL_HYBRID" if (self.CP.flags & ToyotaFlags.HYBRID) else "GAS_PEDAL" + ret.gas = cp.vl[msg]["GAS_PEDAL"] ret.gasPressed = cp.vl["PCM_CRUISE"]["GAS_RELEASED"] == 0 ret.wheelSpeeds = self.get_wheel_speeds( @@ -87,16 +90,16 @@ def update(self, cp, cp_cam): can_gear = int(cp.vl["GEAR_PACKET"]["GEAR"]) ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(can_gear, None)) - ret.leftBlinker = cp.vl["STEERING_LEVERS"]["TURN_SIGNALS"] == 1 - ret.rightBlinker = cp.vl["STEERING_LEVERS"]["TURN_SIGNALS"] == 2 + ret.leftBlinker = cp.vl["BLINKERS_STATE"]["TURN_SIGNALS"] == 1 + ret.rightBlinker = cp.vl["BLINKERS_STATE"]["TURN_SIGNALS"] == 2 ret.steeringTorque = cp.vl["STEER_TORQUE_SENSOR"]["STEER_TORQUE_DRIVER"] - ret.steeringTorqueEps = cp.vl["STEER_TORQUE_SENSOR"]["STEER_TORQUE_EPS"] + ret.steeringTorqueEps = cp.vl["STEER_TORQUE_SENSOR"]["STEER_TORQUE_EPS"] * self.eps_torque_scale # we could use the override bit from dbc, but it's triggered at too high torque values ret.steeringPressed = abs(ret.steeringTorque) > STEER_THRESHOLD - ret.steerWarning = cp.vl["EPS_STATUS"]["LKA_STATE"] not in [1, 5] + ret.steerWarning = cp.vl["EPS_STATUS"]["LKA_STATE"] not in (1, 5) - if self.CP.carFingerprint in [CAR.LEXUS_IS, CAR.LEXUS_RC]: + if self.CP.carFingerprint in (CAR.LEXUS_IS, CAR.LEXUS_RC): ret.cruiseState.available = cp.vl["DSU_CRUISE"]["MAIN_ON"] != 0 ret.cruiseState.speed = cp.vl["DSU_CRUISE"]["SET_SPEED"] * CV.KPH_TO_MS else: @@ -123,7 +126,7 @@ def update(self, cp, cp_cam): # these cars are identified by an ACC_TYPE value of 2. # TODO: it is possible to avoid the lockout and gain stop and go if you # send your own ACC_CONTROL msg on startup with ACC_TYPE set to 1 - if (self.CP.carFingerprint not in TSS2_CAR and self.CP.carFingerprint not in [CAR.LEXUS_IS, CAR.LEXUS_RC]) or \ + if (self.CP.carFingerprint not in TSS2_CAR and self.CP.carFingerprint not in (CAR.LEXUS_IS, CAR.LEXUS_RC)) or \ (self.CP.carFingerprint in TSS2_CAR and self.acc_type == 1): self.low_speed_lockout = cp.vl["PCM_CRUISE_2"]["LOW_SPEED_LOCKOUT"] == 2 @@ -135,7 +138,7 @@ def update(self, cp, cp_cam): else: ret.cruiseState.standstill = self.pcm_acc_status == 7 ret.cruiseState.enabled = bool(cp.vl["PCM_CRUISE"]["CRUISE_ACTIVE"]) - ret.cruiseState.nonAdaptive = cp.vl["PCM_CRUISE"]["CRUISE_STATE"] in [1, 2, 3, 4, 5, 6] + ret.cruiseState.nonAdaptive = cp.vl["PCM_CRUISE"]["CRUISE_STATE"] in (1, 2, 3, 4, 5, 6) ret.genericToggle = bool(cp.vl["LIGHT_STALK"]["AUTO_HIGH_BEAM"]) ret.stockAeb = bool(cp_cam.vl["PRE_COLLISION"]["PRECOLLISION_ACTIVE"] and cp_cam.vl["PRE_COLLISION"]["FORCE"] < -1e-5) @@ -152,47 +155,44 @@ def update(self, cp, cp_cam): @staticmethod def get_can_parser(CP): - signals = [ - # sig_name, sig_address, default - ("STEER_ANGLE", "STEER_ANGLE_SENSOR", 0), - ("GEAR", "GEAR_PACKET", 0), - ("BRAKE_PRESSED", "BRAKE_MODULE", 0), - ("GAS_PEDAL", "GAS_PEDAL", 0), - ("WHEEL_SPEED_FL", "WHEEL_SPEEDS", 0), - ("WHEEL_SPEED_FR", "WHEEL_SPEEDS", 0), - ("WHEEL_SPEED_RL", "WHEEL_SPEEDS", 0), - ("WHEEL_SPEED_RR", "WHEEL_SPEEDS", 0), - ("DOOR_OPEN_FL", "SEATS_DOORS", 1), - ("DOOR_OPEN_FR", "SEATS_DOORS", 1), - ("DOOR_OPEN_RL", "SEATS_DOORS", 1), - ("DOOR_OPEN_RR", "SEATS_DOORS", 1), - ("SEATBELT_DRIVER_UNLATCHED", "SEATS_DOORS", 1), - ("TC_DISABLED", "ESP_CONTROL", 1), - ("BRAKE_HOLD_ACTIVE", "ESP_CONTROL", 1), - ("STEER_FRACTION", "STEER_ANGLE_SENSOR", 0), - ("STEER_RATE", "STEER_ANGLE_SENSOR", 0), - ("CRUISE_ACTIVE", "PCM_CRUISE", 0), - ("CRUISE_STATE", "PCM_CRUISE", 0), - ("GAS_RELEASED", "PCM_CRUISE", 1), - ("STEER_TORQUE_DRIVER", "STEER_TORQUE_SENSOR", 0), - ("STEER_TORQUE_EPS", "STEER_TORQUE_SENSOR", 0), - ("STEER_ANGLE", "STEER_TORQUE_SENSOR", 0), - ("TURN_SIGNALS", "STEERING_LEVERS", 3), # 3 is no blinkers - ("LKA_STATE", "EPS_STATUS", 0), - ("AUTO_HIGH_BEAM", "LIGHT_STALK", 0), - ("DISTANCE_LINES", "PCM_CRUISE_SM", 0), + # sig_name, sig_address + ("STEER_ANGLE", "STEER_ANGLE_SENSOR"), + ("GEAR", "GEAR_PACKET"), + ("BRAKE_PRESSED", "BRAKE_MODULE"), + ("WHEEL_SPEED_FL", "WHEEL_SPEEDS"), + ("WHEEL_SPEED_FR", "WHEEL_SPEEDS"), + ("WHEEL_SPEED_RL", "WHEEL_SPEEDS"), + ("WHEEL_SPEED_RR", "WHEEL_SPEEDS"), + ("DOOR_OPEN_FL", "BODY_CONTROL_STATE"), + ("DOOR_OPEN_FR", "BODY_CONTROL_STATE"), + ("DOOR_OPEN_RL", "BODY_CONTROL_STATE"), + ("DOOR_OPEN_RR", "BODY_CONTROL_STATE"), + ("SEATBELT_DRIVER_UNLATCHED", "BODY_CONTROL_STATE"), + ("TC_DISABLED", "ESP_CONTROL"), + ("BRAKE_HOLD_ACTIVE", "ESP_CONTROL"), + ("STEER_FRACTION", "STEER_ANGLE_SENSOR"), + ("STEER_RATE", "STEER_ANGLE_SENSOR"), + ("CRUISE_ACTIVE", "PCM_CRUISE"), + ("CRUISE_STATE", "PCM_CRUISE"), + ("GAS_RELEASED", "PCM_CRUISE"), + ("STEER_TORQUE_DRIVER", "STEER_TORQUE_SENSOR"), + ("STEER_TORQUE_EPS", "STEER_TORQUE_SENSOR"), + ("STEER_ANGLE", "STEER_TORQUE_SENSOR"), + ("TURN_SIGNALS", "BLINKERS_STATE"), + ("LKA_STATE", "EPS_STATUS"), + ("AUTO_HIGH_BEAM", "LIGHT_STALK"), + ("DISTANCE_LINES", "PCM_CRUISE_SM"), ] checks = [ ("GEAR_PACKET", 1), ("LIGHT_STALK", 1), - ("STEERING_LEVERS", 0.15), - ("SEATS_DOORS", 3), + ("BLINKERS_STATE", 0.15), + ("BODY_CONTROL_STATE", 3), ("ESP_CONTROL", 3), ("EPS_STATUS", 25), ("BRAKE_MODULE", 40), - ("GAS_PEDAL", 33), ("WHEEL_SPEEDS", 80), ("STEER_ANGLE_SENSOR", 80), ("PCM_CRUISE", 33), @@ -200,14 +200,21 @@ def get_can_parser(CP): ("PCM_CRUISE_SM", 1), ] - if CP.carFingerprint in [CAR.LEXUS_IS, CAR.LEXUS_RC]: - signals.append(("MAIN_ON", "DSU_CRUISE", 0)) - signals.append(("SET_SPEED", "DSU_CRUISE", 0)) + if CP.flags & ToyotaFlags.HYBRID: + signals.append(("GAS_PEDAL", "GAS_PEDAL_HYBRID")) + checks.append(("GAS_PEDAL_HYBRID", 33)) + else: + signals.append(("GAS_PEDAL", "GAS_PEDAL")) + checks.append(("GAS_PEDAL", 33)) + + if CP.carFingerprint in (CAR.LEXUS_IS, CAR.LEXUS_RC): + signals.append(("MAIN_ON", "DSU_CRUISE")) + signals.append(("SET_SPEED", "DSU_CRUISE")) checks.append(("DSU_CRUISE", 5)) else: - signals.append(("MAIN_ON", "PCM_CRUISE_2", 0)) - signals.append(("SET_SPEED", "PCM_CRUISE_2", 0)) - signals.append(("LOW_SPEED_LOCKOUT", "PCM_CRUISE_2", 0)) + signals.append(("MAIN_ON", "PCM_CRUISE_2")) + signals.append(("SET_SPEED", "PCM_CRUISE_2")) + signals.append(("LOW_SPEED_LOCKOUT", "PCM_CRUISE_2")) checks.append(("PCM_CRUISE_2", 33)) if CP.hasZss: @@ -216,20 +223,18 @@ def get_can_parser(CP): # add gas interceptor reading if we are using it if CP.enableGasInterceptor: - signals.append(("INTERCEPTOR_GAS", "GAS_SENSOR", 0)) - signals.append(("INTERCEPTOR_GAS2", "GAS_SENSOR", 0)) + signals.append(("INTERCEPTOR_GAS", "GAS_SENSOR")) + signals.append(("INTERCEPTOR_GAS2", "GAS_SENSOR")) checks.append(("GAS_SENSOR", 50)) if CP.enableBsm: signals += [ - ("L_ADJACENT", "BSM", 0), - ("L_APPROACHING", "BSM", 0), - ("R_ADJACENT", "BSM", 0), - ("R_APPROACHING", "BSM", 0), - ] - checks += [ - ("BSM", 1) + ("L_ADJACENT", "BSM"), + ("L_APPROACHING", "BSM"), + ("R_ADJACENT", "BSM"), + ("R_APPROACHING", "BSM"), ] + checks.append(("BSM", 1)) if CP.smartDsu: signals.append(("FD_BUTTON", "SDSU", 0)) @@ -239,10 +244,9 @@ def get_can_parser(CP): @staticmethod def get_cam_can_parser(CP): - signals = [ - ("FORCE", "PRE_COLLISION", 0), - ("PRECOLLISION_ACTIVE", "PRE_COLLISION", 0), + ("FORCE", "PRE_COLLISION"), + ("PRECOLLISION_ACTIVE", "PRE_COLLISION"), ] # use steering message to check if panda is connected to frc @@ -252,8 +256,8 @@ def get_cam_can_parser(CP): ] if CP.carFingerprint in TSS2_CAR: - signals.append(("ACC_TYPE", "ACC_CONTROL", 0)) - signals.append(("DISTANCE", "ACC_CONTROL", 0)) + signals.append(("ACC_TYPE", "ACC_CONTROL")) + signals.append(("DISTANCE", "ACC_CONTROL")) checks.append(("ACC_CONTROL", 33)) return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 2) diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index b68648a9a03d4c..6297168d3d93e8 100755 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -3,7 +3,7 @@ from cereal import car from selfdrive.config import Conversions as CV from selfdrive.car.toyota.tunes import LatTunes, LongTunes, set_long_tune, set_lat_tune -from selfdrive.car.toyota.values import Ecu, CAR, TSS2_CAR, NO_DSU_CAR, MIN_ACC_SPEED, CarControllerParams +from selfdrive.car.toyota.values import Ecu, CAR, ToyotaFlags, TSS2_CAR, NO_DSU_CAR, MIN_ACC_SPEED, EPS_SCALE, CarControllerParams from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint, get_safety_config from selfdrive.car.interfaces import CarInterfaceBase from common.op_params import opParams @@ -24,6 +24,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[]): # py ret.carName = "toyota" ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.toyota)] + ret.safetyConfigs[0].safetyParam = EPS_SCALE[candidate] ret.steerActuatorDelay = 0.12 # Default delay, Prius has larger delay ret.steerLimitTimer = 0.4 @@ -32,8 +33,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[]): # py ret.stoppingControl = False # Toyota starts braking more when it thinks you want to stop - # Most cars use this default safety param - ret.safetyConfigs[0].safetyParam = 73 + stop_and_go = False op_params = opParams() lat_params = LatParams( @@ -47,7 +47,6 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[]): # py ) if candidate == CAR.PRIUS: - ret.safetyConfigs[0].safetyParam = 66 # see conversion factor for STEER_TORQUE_EPS in dbc file stop_and_go = True ret.wheelbase = 2.70 ret.steerRatio = 15.74 # unknown end-to-end spec @@ -57,7 +56,15 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[]): # py set_lat_tune(ret.lateralTuning, lat_params, LatTunes.INDI_PRIUS) ret.steerActuatorDelay = 0.3 - elif candidate in [CAR.RAV4, CAR.RAV4H]: + elif candidate == CAR.PRIUS_V: + stop_and_go = True + ret.wheelbase = 2.78 + ret.steerRatio = 17.4 + tire_stiffness_factor = 0.5533 + ret.mass = 4387. * CV.LB_TO_KG + STD_CARGO_KG + set_lat_tune(ret.lateralTuning, lat_params, LatTunes.LQR_RAV4) + + elif candidate in (CAR.RAV4, CAR.RAV4H): stop_and_go = True if (candidate in CAR.RAV4H) else False ret.wheelbase = 2.65 ret.steerRatio = 16.88 # 14.5 is spec end-to-end @@ -66,15 +73,13 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[]): # py set_lat_tune(ret.lateralTuning, lat_params, LatTunes.LQR_RAV4) elif candidate == CAR.COROLLA: - ret.safetyConfigs[0].safetyParam = 88 - stop_and_go = False ret.wheelbase = 2.70 ret.steerRatio = 17.43 tire_stiffness_factor = 0.444 # not optimized yet ret.mass = 2860. * CV.LB_TO_KG + STD_CARGO_KG # mean between normal and hybrid set_lat_tune(ret.lateralTuning, lat_params, LatTunes.STEER_MODEL_COROLLA) - elif candidate in [CAR.LEXUS_RX, CAR.LEXUS_RXH, CAR.LEXUS_RX_TSS2, CAR.LEXUS_RXH_TSS2]: + elif candidate in (CAR.LEXUS_RX, CAR.LEXUS_RXH, CAR.LEXUS_RX_TSS2, CAR.LEXUS_RXH_TSS2): stop_and_go = True ret.wheelbase = 2.79 ret.steerRatio = 16. # 14.8 is spec end-to-end @@ -83,7 +88,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[]): # py ret.mass = 4481. * CV.LB_TO_KG + STD_CARGO_KG # mean between min and max set_lat_tune(ret.lateralTuning, lat_params, LatTunes.PID_C) - elif candidate in [CAR.CHR, CAR.CHRH]: + elif candidate in (CAR.CHR, CAR.CHRH): stop_and_go = True ret.wheelbase = 2.63906 ret.steerRatio = 13.6 @@ -91,7 +96,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[]): # py ret.mass = 3300. * CV.LB_TO_KG + STD_CARGO_KG set_lat_tune(ret.lateralTuning, lat_params, LatTunes.PID_F) - elif candidate in [CAR.CAMRY, CAR.CAMRYH, CAR.CAMRY_TSS2, CAR.CAMRYH_TSS2]: + elif candidate in (CAR.CAMRY, CAR.CAMRYH, CAR.CAMRY_TSS2, CAR.CAMRYH_TSS2): stop_and_go = True ret.wheelbase = 2.82448 ret.steerRatio = 13.7 @@ -100,7 +105,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[]): # py # set_lat_tune(ret.lateralTuning, lat_params, LatTunes.STEER_MODEL_CAMRY) set_lat_tune(ret.lateralTuning, lat_params, LatTunes.PID_H) - elif candidate in [CAR.HIGHLANDER_TSS2, CAR.HIGHLANDERH_TSS2]: + elif candidate in (CAR.HIGHLANDER_TSS2, CAR.HIGHLANDERH_TSS2): stop_and_go = True ret.wheelbase = 2.84988 # 112.2 in = 2.84988 m ret.steerRatio = 16.0 @@ -108,7 +113,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[]): # py ret.mass = 4700. * CV.LB_TO_KG + STD_CARGO_KG # 4260 + 4-5 people set_lat_tune(ret.lateralTuning, lat_params, LatTunes.PID_G) - elif candidate in [CAR.HIGHLANDER, CAR.HIGHLANDERH]: + elif candidate in (CAR.HIGHLANDER, CAR.HIGHLANDERH): stop_and_go = True ret.wheelbase = 2.78 ret.steerRatio = 16.0 @@ -116,15 +121,14 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[]): # py ret.mass = 4607. * CV.LB_TO_KG + STD_CARGO_KG # mean between normal and hybrid limited set_lat_tune(ret.lateralTuning, lat_params, LatTunes.PID_G) - elif candidate in [CAR.AVALON, CAR.AVALON_2019, CAR.AVALONH_2019, CAR.AVALON_TSS2]: - stop_and_go = False + elif candidate in (CAR.AVALON, CAR.AVALON_2019, CAR.AVALONH_2019, CAR.AVALON_TSS2): ret.wheelbase = 2.82 ret.steerRatio = 14.8 # Found at https://pressroom.toyota.com/releases/2016+avalon+product+specs.download tire_stiffness_factor = 0.7983 ret.mass = 3505. * CV.LB_TO_KG + STD_CARGO_KG # mean between normal and hybrid set_lat_tune(ret.lateralTuning, lat_params, LatTunes.PID_H) - elif candidate in [CAR.RAV4_TSS2, CAR.RAV4H_TSS2]: + elif candidate in (CAR.RAV4_TSS2, CAR.RAV4H_TSS2): stop_and_go = True ret.wheelbase = 2.68986 ret.steerRatio = 14.3 @@ -151,7 +155,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[]): # py ret.lateralTuning.indi.actuatorEffectivenessV = [15, 15] ret.steerRateCost = 0.3 - elif candidate in [CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2]: + elif candidate in (CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2): stop_and_go = True ret.wheelbase = 2.67 # Average between 2.70 for sedan and 2.64 for hatchback ret.steerRatio = 13.9 @@ -172,20 +176,12 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[]): # py else: set_lat_tune(ret.lateralTuning, lat_params, LatTunes.PID_D) - elif candidate in [CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2]: + elif candidate in (CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2, CAR.LEXUS_ESH): stop_and_go = True ret.wheelbase = 2.8702 ret.steerRatio = 16.0 # not optimized tire_stiffness_factor = 0.444 # not optimized yet - ret.mass = 3704. * CV.LB_TO_KG + STD_CARGO_KG - set_lat_tune(ret.lateralTuning, lat_params, LatTunes.PID_D) - - elif candidate == CAR.LEXUS_ESH: - stop_and_go = True - ret.wheelbase = 2.8190 - ret.steerRatio = 16.06 - tire_stiffness_factor = 0.444 # not optimized yet - ret.mass = 3682. * CV.LB_TO_KG + STD_CARGO_KG + ret.mass = 3677. * CV.LB_TO_KG + STD_CARGO_KG # mean between min and max set_lat_tune(ret.lateralTuning, lat_params, LatTunes.PID_D) elif candidate == CAR.SIENNA: @@ -196,26 +192,14 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[]): # py ret.mass = 4590. * CV.LB_TO_KG + STD_CARGO_KG set_lat_tune(ret.lateralTuning, lat_params, LatTunes.PID_J) - elif candidate == CAR.LEXUS_IS: - ret.safetyConfigs[0].safetyParam = 77 - stop_and_go = False + elif candidate in (CAR.LEXUS_IS, CAR.LEXUS_RC): ret.wheelbase = 2.79908 ret.steerRatio = 13.3 tire_stiffness_factor = 0.444 ret.mass = 3736.8 * CV.LB_TO_KG + STD_CARGO_KG set_lat_tune(ret.lateralTuning, lat_params, LatTunes.PID_L) - elif candidate == CAR.LEXUS_RC: - ret.safetyConfigs[0].safetyParam = 77 - stop_and_go = False - ret.wheelbase = 2.73050 - ret.steerRatio = 13.3 - tire_stiffness_factor = 0.444 - ret.mass = 3736.8 * CV.LB_TO_KG + STD_CARGO_KG - set_lat_tune(ret.lateralTuning, lat_params, LatTunes.PID_L) - elif candidate == CAR.LEXUS_CTH: - ret.safetyConfigs[0].safetyParam = 100 stop_and_go = True ret.wheelbase = 2.60 ret.steerRatio = 18.6 @@ -223,7 +207,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[]): # py ret.mass = 3108 * CV.LB_TO_KG + STD_CARGO_KG # mean between min and max set_lat_tune(ret.lateralTuning, lat_params, LatTunes.PID_M) - elif candidate in [CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.LEXUS_NX_TSS2]: + elif candidate in (CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.LEXUS_NX_TSS2): stop_and_go = True ret.wheelbase = 2.66 ret.steerRatio = 14.7 @@ -276,14 +260,13 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[]): # py # if the smartDSU is detected, openpilot can send ACC_CMD (and the smartDSU will block it from the DSU) or not (the DSU is "connected") ret.openpilotLongitudinalControl = ret.smartDsu or ret.enableDsu or candidate in TSS2_CAR + if 0x245 in fingerprint[0]: + ret.flags |= ToyotaFlags.HYBRID.value + # min speed to enable ACC. if car can do stop and go, then set enabling speed # to a negative value, so it won't matter. ret.minEnableSpeed = -1. if (stop_and_go or ret.enableGasInterceptor) else MIN_ACC_SPEED - # removing the DSU disables AEB and it's considered a community maintained feature - # intercepting the DSU is a community feature since it requires unofficial hardware - ret.communityFeature = ret.enableGasInterceptor or ret.enableDsu or ret.smartDsu - if ret.enableGasInterceptor: set_long_tune(ret.longitudinalTuning, LongTunes.PEDAL) elif candidate in TSS2_CAR: diff --git a/selfdrive/car/toyota/radar_interface.py b/selfdrive/car/toyota/radar_interface.py index a7d2ec37bb26db..590840851d87e3 100755 --- a/selfdrive/car/toyota/radar_interface.py +++ b/selfdrive/car/toyota/radar_interface.py @@ -4,6 +4,7 @@ from selfdrive.car.toyota.values import NO_DSU_CAR, DBC, TSS2_CAR from selfdrive.car.interfaces import RadarInterfaceBase + def _create_radar_can_parser(car_fingerprint): if car_fingerprint in TSS2_CAR: RADAR_A_MSGS = list(range(0x180, 0x190)) @@ -16,11 +17,10 @@ def _create_radar_can_parser(car_fingerprint): msg_b_n = len(RADAR_B_MSGS) signals = list(zip(['LONG_DIST'] * msg_a_n + ['NEW_TRACK'] * msg_a_n + ['LAT_DIST'] * msg_a_n + - ['REL_SPEED'] * msg_a_n + ['VALID'] * msg_a_n + ['SCORE'] * msg_b_n, - RADAR_A_MSGS * 5 + RADAR_B_MSGS, - [255] * msg_a_n + [1] * msg_a_n + [0] * msg_a_n + [0] * msg_a_n + [0] * msg_a_n + [0] * msg_b_n)) + ['REL_SPEED'] * msg_a_n + ['VALID'] * msg_a_n + ['SCORE'] * msg_b_n, + RADAR_A_MSGS * 5 + RADAR_B_MSGS)) - checks = list(zip(RADAR_A_MSGS + RADAR_B_MSGS, [20]*(msg_a_n + msg_b_n))) + checks = list(zip(RADAR_A_MSGS + RADAR_B_MSGS, [20] * (msg_a_n + msg_b_n))) return CANParser(DBC[car_fingerprint]['radar'], signals, checks, 1) diff --git a/selfdrive/car/toyota/toyotacan.py b/selfdrive/car/toyota/toyotacan.py index 83702ebe26c995..038f699575b45c 100644 --- a/selfdrive/car/toyota/toyotacan.py +++ b/selfdrive/car/toyota/toyotacan.py @@ -69,17 +69,32 @@ def create_fcw_command(packer, fcw): def create_ui_command(packer, steer, chime, left_line, right_line, left_lane_depart, right_lane_depart, enabled): values = { + "TWO_BEEPS": chime, + "LDA_ALERT": steer, "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, "BARRIERS" : 1 if enabled else 0, - "SET_ME_X0C": 0x0c, - "SET_ME_X2C": 0x2c, - "SET_ME_X38": 0x38, - "SET_ME_X02": 0x02, + + # static signals + "SET_ME_X02": 2, "SET_ME_X01": 1, - "SET_ME_X01_2": 1, + "LKAS_STATUS": 1, "REPEATED_BEEPS": 0, - "TWO_BEEPS": chime, - "LDA_ALERT": steer, + "LANE_SWAY_FLD": 7, + "LANE_SWAY_BUZZER": 0, + "LANE_SWAY_WARNING": 0, + "LDA_FRONT_CAMERA_BLOCKED": 0, + "TAKE_CONTROL": 0, + "LANE_SWAY_SENSITIVITY": 2, + "LANE_SWAY_TOGGLE": 1, + "LDA_ON_MESSAGE": 0, + "LDA_SPEED_TOO_LOW": 0, + "LDA_SA_TOGGLE": 1, + "LDA_SENSITIVITY": 2, + "LDA_UNAVAILABLE": 0, + "LDA_MALFUNCTION": 0, + "LDA_UNAVAILABLE_QUIET": 0, + "ADJUSTING_CAMERA": 0, + "LDW_EXIST": 1, } return packer.make_can_msg("LKAS_HUD", 0, values) diff --git a/selfdrive/car/toyota/values.py b/selfdrive/car/toyota/values.py index 767bc7ef3ee29e..2901ef5ec41302 100644 --- a/selfdrive/car/toyota/values.py +++ b/selfdrive/car/toyota/values.py @@ -1,4 +1,5 @@ -# flake8: noqa +from collections import defaultdict +from enum import IntFlag from cereal import car from selfdrive.car import dbc_dict @@ -18,6 +19,11 @@ class CarControllerParams: STEER_DELTA_DOWN = 25 # always lower than 45 otherwise the Rav4 faults (Prius seems ok with 50) STEER_ERROR_MAX = 350 # max delta between torque cmd and torque motor + +class ToyotaFlags(IntFlag): + HYBRID = 1 + + class CAR: # Toyota ALPHARD_TSS2 = "TOYOTA ALPHARD 2020" @@ -40,6 +46,7 @@ class CAR: HIGHLANDERH = "TOYOTA HIGHLANDER HYBRID 2018" HIGHLANDERH_TSS2 = "TOYOTA HIGHLANDER HYBRID 2020" PRIUS = "TOYOTA PRIUS 2017" + PRIUS_V = "TOYOTA PRIUS v 2017" PRIUS_TSS2 = "TOYOTA PRIUS TSS2 2021" RAV4 = "TOYOTA RAV4 2017" RAV4H = "TOYOTA RAV4 HYBRID 2017" @@ -67,25 +74,24 @@ class CAR: STATIC_DSU_MSGS = [ (0x128, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.AVALON), 1, 3, b'\xf4\x01\x90\x83\x00\x37'), (0x128, (CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ESH), 1, 3, b'\x03\x00\x20\x00\x00\x52'), - (0x141, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ESH, CAR.LEXUS_RX), 1, 2, b'\x00\x00\x00\x46'), - (0x160, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ESH, CAR.LEXUS_RX), 1, 7, b'\x00\x00\x08\x12\x01\x31\x9c\x51'), - (0x161, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.AVALON, CAR.LEXUS_RX), 1, 7, b'\x00\x1e\x00\x00\x00\x80\x07'), + (0x141, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ESH, CAR.LEXUS_RX, CAR.PRIUS_V), 1, 2, b'\x00\x00\x00\x46'), + (0x160, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ESH, CAR.LEXUS_RX, CAR.PRIUS_V), 1, 7, b'\x00\x00\x08\x12\x01\x31\x9c\x51'), + (0x161, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.AVALON, CAR.LEXUS_RX, CAR.PRIUS_V), 1, 7, b'\x00\x1e\x00\x00\x00\x80\x07'), (0X161, (CAR.HIGHLANDERH, CAR.HIGHLANDER, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ESH), 1, 7, b'\x00\x1e\x00\xd4\x00\x00\x5b'), - (0x283, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ESH, CAR.LEXUS_RX), 0, 3, b'\x00\x00\x00\x00\x00\x00\x8c'), + (0x283, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ESH, CAR.LEXUS_RX, CAR.PRIUS_V), 0, 3, b'\x00\x00\x00\x00\x00\x00\x8c'), (0x2E6, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH), 0, 3, b'\xff\xf8\x00\x08\x7f\xe0\x00\x4e'), (0x2E7, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH), 0, 3, b'\xa8\x9c\x31\x9c\x00\x00\x00\x02'), (0x33E, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH), 0, 20, b'\x0f\xff\x26\x40\x00\x1f\x00'), - (0x344, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ESH, CAR.LEXUS_RX), 0, 5, b'\x00\x00\x01\x00\x00\x00\x00\x50'), + (0x344, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ESH, CAR.LEXUS_RX, CAR.PRIUS_V), 0, 5, b'\x00\x00\x01\x00\x00\x00\x00\x50'), (0x365, (CAR.PRIUS, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.HIGHLANDERH), 0, 20, b'\x00\x00\x00\x80\x03\x00\x08'), - (0x365, (CAR.RAV4, CAR.RAV4H, CAR.COROLLA, CAR.HIGHLANDER, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ESH, CAR.LEXUS_RX), 0, 20, b'\x00\x00\x00\x80\xfc\x00\x08'), + (0x365, (CAR.RAV4, CAR.RAV4H, CAR.COROLLA, CAR.HIGHLANDER, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ESH, CAR.LEXUS_RX, CAR.PRIUS_V), 0, 20, b'\x00\x00\x00\x80\xfc\x00\x08'), (0x366, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.HIGHLANDERH), 0, 20, b'\x00\x00\x4d\x82\x40\x02\x00'), - (0x366, (CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ESH, CAR.LEXUS_RX), 0, 20, b'\x00\x72\x07\xff\x09\xfe\x00'), + (0x366, (CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ESH, CAR.LEXUS_RX, CAR.PRIUS_V), 0, 20, b'\x00\x72\x07\xff\x09\xfe\x00'), (0x470, (CAR.PRIUS, CAR.LEXUS_RXH), 1, 100, b'\x00\x00\x02\x7a'), - (0x470, (CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.RAV4H, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ESH), 1, 100, b'\x00\x00\x01\x79'), - (0x4CB, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDERH, CAR.HIGHLANDER, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ESH, CAR.LEXUS_RX), 0, 100, b'\x0c\x00\x00\x00\x00\x00\x00\x00'), + (0x470, (CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.RAV4H, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ESH, CAR.PRIUS_V), 1, 100, b'\x00\x00\x01\x79'), + (0x4CB, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDERH, CAR.HIGHLANDER, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ESH, CAR.LEXUS_RX, CAR.PRIUS_V), 0, 100, b'\x0c\x00\x00\x00\x00\x00\x00\x00'), ] - FW_VERSIONS = { CAR.AVALON: { (Ecu.esp, 0x7b0, None): [ @@ -362,12 +368,14 @@ class CAR: b'\x018966306Q5000\x00\x00\x00\x00', b'\x018966306T3100\x00\x00\x00\x00', b'\x018966306T3200\x00\x00\x00\x00', + b'\x018966306T4000\x00\x00\x00\x00', b'\x018966306T4100\x00\x00\x00\x00', ], (Ecu.fwdRadar, 0x750, 0xf): [ b'\x018821F6201200\x00\x00\x00\x00', ], (Ecu.fwdCamera, 0x750, 0x6d): [ + b'\x028646F0602100\x00\x00\x00\x008646G5301200\x00\x00\x00\x00', b'\x028646F0602200\x00\x00\x00\x008646G5301200\x00\x00\x00\x00', b'\x028646F3305200\x00\x00\x00\x008646G5301200\x00\x00\x00\x00', b'\x028646F3305300\x00\x00\x00\x008646G5301200\x00\x00\x00\x00', @@ -394,6 +402,7 @@ class CAR: }, CAR.CHR: { (Ecu.engine, 0x700, None): [ + b'\x01896631021100\x00\x00\x00\x00', b'\x01896631017100\x00\x00\x00\x00', b'\x01896631017200\x00\x00\x00\x00', b'\x0189663F413100\x00\x00\x00\x00', @@ -544,6 +553,7 @@ class CAR: b'\x01896630ZG5100\x00\x00\x00\x00', b'\x01896630ZG5200\x00\x00\x00\x00', b'\x01896630ZG5300\x00\x00\x00\x00', + b'\x01896630ZP1000\x00\x00\x00\x00', b'\x01896630ZP2000\x00\x00\x00\x00', b'\x01896630ZQ5000\x00\x00\x00\x00', b'\x018966312L8000\x00\x00\x00\x00', @@ -576,6 +586,7 @@ class CAR: b'\x03312N6000\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00895231203402\x00\x00\x00\x00', b'\x03312N6100\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00895231203302\x00\x00\x00\x00', b'\x03312N6100\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00895231203402\x00\x00\x00\x00', + b'\x02312K4000\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00', ], (Ecu.eps, 0x7a1, None): [ b'\x018965B12350\x00\x00\x00\x00\x00\x00', @@ -587,6 +598,7 @@ class CAR: b'\x018965B1255000\x00\x00\x00\x00', b'8965B12361\x00\x00\x00\x00\x00\x00', b'8965B16011\x00\x00\x00\x00\x00\x00', + b'\x018965B12510\x00\x00\x00\x00\x00\x00' ], (Ecu.esp, 0x7b0, None): [ b'\x01F152602280\x00\x00\x00\x00\x00\x00', @@ -601,11 +613,13 @@ class CAR: b'\x01F152612B51\x00\x00\x00\x00\x00\x00', b'\x01F152612B60\x00\x00\x00\x00\x00\x00', b'\x01F152612B61\x00\x00\x00\x00\x00\x00', + b'\x01F152612B62\x00\x00\x00\x00\x00\x00', b'\x01F152612B71\x00\x00\x00\x00\x00\x00', b'\x01F152612B81\x00\x00\x00\x00\x00\x00', b'\x01F152612B90\x00\x00\x00\x00\x00\x00', b'\x01F152612C00\x00\x00\x00\x00\x00\x00', b'F152602191\x00\x00\x00\x00\x00\x00', + b'\x01F152612862\x00\x00\x00\x00\x00\x00', ], (Ecu.fwdRadar, 0x750, 0xf): [ b'\x018821F3301100\x00\x00\x00\x00', @@ -810,9 +824,11 @@ class CAR: b'\x01F15264872300\x00\x00\x00\x00', b'\x01F15264872400\x00\x00\x00\x00', b'\x01F15264872500\x00\x00\x00\x00', + b'\x01F15264873500\x00\x00\x00\x00', b'\x01F152648C6300\x00\x00\x00\x00', ], (Ecu.engine, 0x700, None): [ + b'\x01896630E67000\x00\x00\x00\x00', b'\x01896630EA1000\x00\x00\x00\x00', b'\x01896630EE4000\x00\x00\x00\x00', b'\x01896630EA1000\x00\x00\x00\x00897CF4801001\x00\x00\x00\x00', @@ -970,6 +986,23 @@ class CAR: b'8646F4705200\x00\x00\x00\x00', ], }, + CAR.PRIUS_V: { + (Ecu.esp, 0x7b0, None): [ + b'F152647280\x00\x00\x00\x00\x00\x00', + ], + (Ecu.engine, 0x7e0, None): [ + b'\x0234781000\x00\x00\x00\x00\x00\x00\x00\x00A4701000\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.dsu, 0x791, None): [ + b'881514705100\x00\x00\x00\x00', + ], + (Ecu.fwdRadar, 0x750, 0xf): [ + b'8821F4702300\x00\x00\x00\x00', + ], + (Ecu.fwdCamera, 0x750, 0x6d): [ + b'8646F4703300\x00\x00\x00\x00', + ], + }, CAR.RAV4: { (Ecu.engine, 0x7e0, None): [ b'\x02342Q1000\x00\x00\x00\x00\x00\x00\x00\x0054212000\x00\x00\x00\x00\x00\x00\x00\x00', @@ -1522,20 +1555,21 @@ class CAR: }, CAR.LEXUS_RX_TSS2: { (Ecu.engine, 0x700, None): [ - b'\x01896630EC9000\x00\x00\x00\x00', - b'\x01896634D12000\x00\x00\x00\x00', - b'\x01896630EB0000\x00\x00\x00\x00', b'\x01896630EA9000\x00\x00\x00\x00', + b'\x01896630EB0000\x00\x00\x00\x00', + b'\x01896630EC9000\x00\x00\x00\x00', b'\x01896630ED0000\x00\x00\x00\x00', + b'\x01896630ED6000\x00\x00\x00\x00', b'\x018966348W5100\x00\x00\x00\x00', b'\x018966348W9000\x00\x00\x00\x00', + b'\x01896634D12000\x00\x00\x00\x00', b'\x01896634D12100\x00\x00\x00\x00', ], (Ecu.esp, 0x7b0, None): [ - b'\x01F152648801\x00\x00\x00\x00\x00\x00', b'\x01F15260E031\x00\x00\x00\x00\x00\x00', b'\x01F15260E041\x00\x00\x00\x00\x00\x00', b'\x01F152648781\x00\x00\x00\x00\x00\x00', + b'\x01F152648801\x00\x00\x00\x00\x00\x00', ], (Ecu.eps, 0x7a1, None): [ b'8965B48261\x00\x00\x00\x00\x00\x00', @@ -1547,8 +1581,9 @@ class CAR: b'\x018821F3301400\x00\x00\x00\x00', ], (Ecu.fwdCamera, 0x750, 0x6d): [ - b'\x028646F4810200\x00\x00\x00\x008646G2601400\x00\x00\x00\x00', b'\x028646F4810100\x00\x00\x00\x008646G2601200\x00\x00\x00\x00', + b'\x028646F4810200\x00\x00\x00\x008646G2601400\x00\x00\x00\x00', + b'\x028646F4810300\x00\x00\x00\x008646G2601400\x00\x00\x00\x00', ], }, CAR.LEXUS_RXH_TSS2: { @@ -1609,64 +1644,79 @@ class CAR: (Ecu.fwdCamera, 0x750, 0x6d): [b'\x028646F6201400\x00\x00\x00\x008646G5301200\x00\x00\x00\x00',], }, CAR.ALPHARD_TSS2: { - (Ecu.engine, 0x7e0, None): [b'\x0235883000\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00',], - (Ecu.eps, 0x7a1, None): [b'8965B58040\x00\x00\x00\x00\x00\x00',], - (Ecu.fwdRadar, 0x750, 0xf): [b'\x018821F3301400\x00\x00\x00\x00',], - (Ecu.fwdCamera, 0x750, 0x6d): [b'\x028646F5803200\x00\x00\x00\x008646G2601400\x00\x00\x00\x00',], + (Ecu.engine, 0x7e0, None): [ + b'\x0235870000\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00', + b'\x0235883000\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.eps, 0x7a1, None): [ + b'8965B58040\x00\x00\x00\x00\x00\x00', + b'8965B58052\x00\x00\x00\x00\x00\x00', + ], + (Ecu.fwdRadar, 0x750, 0xf): [ + b'\x018821F3301200\x00\x00\x00\x00', + b'\x018821F3301400\x00\x00\x00\x00', + ], + (Ecu.fwdCamera, 0x750, 0x6d): [ + b'\x028646F58010C0\x00\x00\x00\x008646G26011A0\x00\x00\x00\x00', + b'\x028646F5803200\x00\x00\x00\x008646G2601400\x00\x00\x00\x00', + ], }, } STEER_THRESHOLD = 100 DBC = { - CAR.RAV4H: dbc_dict('toyota_rav4_hybrid_2017_pt_generated', 'toyota_adas'), - CAR.RAV4: dbc_dict('toyota_rav4_2017_pt_generated', 'toyota_adas'), - CAR.PRIUS: dbc_dict('toyota_prius_2017_pt_generated', 'toyota_adas'), - CAR.COROLLA: dbc_dict('toyota_corolla_2017_pt_generated', 'toyota_adas'), - CAR.LEXUS_RC: dbc_dict('lexus_is_2018_pt_generated', 'toyota_adas'), - CAR.LEXUS_RX: dbc_dict('lexus_rx_350_2016_pt_generated', 'toyota_adas'), - CAR.LEXUS_RXH: dbc_dict('lexus_rx_hybrid_2017_pt_generated', 'toyota_adas'), + CAR.RAV4H: dbc_dict('toyota_tnga_k_pt_generated', 'toyota_adas'), + CAR.RAV4: dbc_dict('toyota_new_mc_pt_generated', 'toyota_adas'), + CAR.PRIUS: dbc_dict('toyota_nodsu_pt_generated', 'toyota_adas'), + CAR.PRIUS_V: dbc_dict('toyota_new_mc_pt_generated', 'toyota_adas'), + CAR.COROLLA: dbc_dict('toyota_new_mc_pt_generated', 'toyota_adas'), + CAR.LEXUS_RC: dbc_dict('toyota_tnga_k_pt_generated', 'toyota_adas'), + CAR.LEXUS_RX: dbc_dict('toyota_tnga_k_pt_generated', 'toyota_adas'), + CAR.LEXUS_RXH: dbc_dict('toyota_tnga_k_pt_generated', 'toyota_adas'), CAR.LEXUS_RX_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'), - CAR.LEXUS_RXH_TSS2: dbc_dict('toyota_nodsu_hybrid_pt_generated', 'toyota_tss2_adas'), + CAR.LEXUS_RXH_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'), CAR.CHR: dbc_dict('toyota_nodsu_pt_generated', 'toyota_adas'), - CAR.CHRH: dbc_dict('toyota_nodsu_hybrid_pt_generated', 'toyota_adas'), + CAR.CHRH: dbc_dict('toyota_nodsu_pt_generated', 'toyota_adas'), CAR.CAMRY: dbc_dict('toyota_nodsu_pt_generated', 'toyota_adas'), - CAR.CAMRYH: dbc_dict('toyota_camry_hybrid_2018_pt_generated', 'toyota_adas'), + CAR.CAMRYH: dbc_dict('toyota_nodsu_pt_generated', 'toyota_adas'), CAR.CAMRY_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'), - CAR.CAMRYH_TSS2: dbc_dict('toyota_nodsu_hybrid_pt_generated', 'toyota_tss2_adas'), - CAR.HIGHLANDER: dbc_dict('toyota_highlander_2017_pt_generated', 'toyota_adas'), + CAR.CAMRYH_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'), + CAR.HIGHLANDER: dbc_dict('toyota_tnga_k_pt_generated', 'toyota_adas'), CAR.HIGHLANDER_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'), - CAR.HIGHLANDERH: dbc_dict('toyota_highlander_hybrid_2018_pt_generated', 'toyota_adas'), - CAR.HIGHLANDERH_TSS2: dbc_dict('toyota_nodsu_hybrid_pt_generated', 'toyota_tss2_adas'), - CAR.AVALON: dbc_dict('toyota_avalon_2017_pt_generated', 'toyota_adas'), + CAR.HIGHLANDERH: dbc_dict('toyota_tnga_k_pt_generated', 'toyota_adas'), + CAR.HIGHLANDERH_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'), + CAR.AVALON: dbc_dict('toyota_tnga_k_pt_generated', 'toyota_adas'), CAR.AVALON_2019: dbc_dict('toyota_nodsu_pt_generated', 'toyota_adas'), - CAR.AVALONH_2019: dbc_dict('toyota_nodsu_hybrid_pt_generated', 'toyota_adas'), + CAR.AVALONH_2019: dbc_dict('toyota_nodsu_pt_generated', 'toyota_adas'), CAR.AVALON_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'), CAR.RAV4_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'), CAR.COROLLA_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'), - CAR.COROLLAH_TSS2: dbc_dict('toyota_nodsu_hybrid_pt_generated', 'toyota_tss2_adas'), + CAR.COROLLAH_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'), CAR.LEXUS_ES_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'), - CAR.LEXUS_ESH_TSS2: dbc_dict('toyota_nodsu_hybrid_pt_generated', 'toyota_tss2_adas'), - CAR.LEXUS_ESH: dbc_dict('lexus_ct200h_2018_pt_generated', 'toyota_adas'), - CAR.SIENNA: dbc_dict('toyota_sienna_xle_2018_pt_generated', 'toyota_adas'), - CAR.LEXUS_IS: dbc_dict('lexus_is_2018_pt_generated', 'toyota_adas'), - CAR.LEXUS_CTH: dbc_dict('lexus_ct200h_2018_pt_generated', 'toyota_adas'), - CAR.RAV4H_TSS2: dbc_dict('toyota_nodsu_hybrid_pt_generated', 'toyota_tss2_adas'), - CAR.LEXUS_NXH: dbc_dict('lexus_nx300h_2018_pt_generated', 'toyota_adas'), - CAR.LEXUS_NX: dbc_dict('lexus_nx300_2018_pt_generated', 'toyota_adas'), + CAR.LEXUS_ESH_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'), + CAR.LEXUS_ESH: dbc_dict('toyota_new_mc_pt_generated', 'toyota_adas'), + CAR.SIENNA: dbc_dict('toyota_tnga_k_pt_generated', 'toyota_adas'), + CAR.LEXUS_IS: dbc_dict('toyota_tnga_k_pt_generated', 'toyota_adas'), + CAR.LEXUS_CTH: dbc_dict('toyota_new_mc_pt_generated', 'toyota_adas'), + CAR.RAV4H_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'), + CAR.LEXUS_NXH: dbc_dict('toyota_tnga_k_pt_generated', 'toyota_adas'), + CAR.LEXUS_NX: dbc_dict('toyota_tnga_k_pt_generated', 'toyota_adas'), CAR.LEXUS_NX_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'), - CAR.PRIUS_TSS2: dbc_dict('toyota_nodsu_hybrid_pt_generated', 'toyota_tss2_adas'), - CAR.MIRAI: dbc_dict('toyota_nodsu_hybrid_pt_generated', 'toyota_tss2_adas'), + CAR.PRIUS_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'), + CAR.MIRAI: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'), CAR.ALPHARD_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'), } +# These cars have non-standard EPS torque scale factors. All others are 73 +EPS_SCALE = defaultdict(lambda: 73, {CAR.PRIUS: 66, CAR.COROLLA: 88, CAR.LEXUS_IS: 77, CAR.LEXUS_RC: 77, CAR.LEXUS_CTH: 100, CAR.PRIUS_V: 100}) # Toyota/Lexus Safety Sense 2.0 and 2.5 TSS2_CAR = {CAR.RAV4_TSS2, CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2, CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2, CAR.RAV4H_TSS2, - CAR.LEXUS_RX_TSS2, CAR.LEXUS_RXH_TSS2, CAR.HIGHLANDER_TSS2, CAR.HIGHLANDERH_TSS2, CAR.PRIUS_TSS2, CAR.CAMRY_TSS2, CAR.CAMRYH_TSS2, - CAR.MIRAI, CAR.LEXUS_NX_TSS2, CAR.ALPHARD_TSS2, CAR.AVALON_TSS2} + CAR.LEXUS_RX_TSS2, CAR.LEXUS_RXH_TSS2, CAR.HIGHLANDER_TSS2, CAR.HIGHLANDERH_TSS2, CAR.PRIUS_TSS2, CAR.CAMRY_TSS2, CAR.CAMRYH_TSS2, + CAR.MIRAI, CAR.LEXUS_NX_TSS2, CAR.ALPHARD_TSS2, CAR.AVALON_TSS2} NO_DSU_CAR = TSS2_CAR | {CAR.CHR, CAR.CHRH, CAR.CAMRY, CAR.CAMRYH} # no resume button press required -NO_STOP_TIMER_CAR = TSS2_CAR | {CAR.RAV4H, CAR.HIGHLANDERH, CAR.HIGHLANDER, CAR.SIENNA, CAR.LEXUS_ESH} +NO_STOP_TIMER_CAR = TSS2_CAR | {CAR.PRIUS_V, CAR.RAV4H, CAR.HIGHLANDERH, CAR.HIGHLANDER, CAR.SIENNA, CAR.LEXUS_ESH} diff --git a/selfdrive/car/volkswagen/carcontroller.py b/selfdrive/car/volkswagen/carcontroller.py index a0119e2a52ed3d..f85a81a538902f 100644 --- a/selfdrive/car/volkswagen/carcontroller.py +++ b/selfdrive/car/volkswagen/carcontroller.py @@ -21,7 +21,7 @@ def __init__(self, dbc_name, CP, VM): self.steer_rate_limited = False - def update(self, enabled, CS, frame, ext_bus, actuators, visual_alert, left_lane_visible, right_lane_visible, left_lane_depart, right_lane_depart): + def update(self, c, enabled, CS, frame, ext_bus, actuators, visual_alert, left_lane_visible, right_lane_visible, left_lane_depart, right_lane_depart): """ Controls thread """ can_sends = [] @@ -39,7 +39,7 @@ def update(self, enabled, CS, frame, ext_bus, actuators, visual_alert, left_lane # torque value. Do that anytime we happen to have 0 torque, or failing that, # when exceeding ~1/3 the 360 second timer. - if enabled and CS.out.vEgo > CS.CP.minSteerSpeed and not (CS.out.standstill or CS.out.steerError or CS.out.steerWarning): + if c.active and CS.out.vEgo > CS.CP.minSteerSpeed and not (CS.out.standstill or CS.out.steerError or CS.out.steerWarning): new_steer = int(round(actuators.steer * P.STEER_MAX)) apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, P) self.steer_rate_limited = new_steer != apply_steer @@ -72,7 +72,7 @@ def update(self, enabled, CS, frame, ext_bus, actuators, visual_alert, left_lane # **** HUD Controls ***************************************************** # if frame % P.LDW_STEP == 0: - if visual_alert in [VisualAlert.steerRequired, VisualAlert.ldw]: + if visual_alert in (VisualAlert.steerRequired, VisualAlert.ldw): hud_alert = MQB_LDW_MESSAGES["laneAssistTakeOverSilent"] else: hud_alert = MQB_LDW_MESSAGES["none"] diff --git a/selfdrive/car/volkswagen/carstate.py b/selfdrive/car/volkswagen/carstate.py index 97e3094fa509e2..1fe8b56b9038e4 100644 --- a/selfdrive/car/volkswagen/carstate.py +++ b/selfdrive/car/volkswagen/carstate.py @@ -33,7 +33,7 @@ def update(self, pt_cp, cam_cp, ext_cp, trans_type): # Update steering angle, rate, yaw rate, and driver input torque. VW send # the sign/direction in a separate signal so they must be recombined. - ret.steeringAngleDeg = pt_cp.vl["LH_EPS_03"]["EPS_Berechneter_LW"] * (1, -1)[int(pt_cp.vl["LH_EPS_03"]["EPS_VZ_BLW"])] + ret.steeringAngleDeg = pt_cp.vl["LWI_01"]["LWI_Lenkradwinkel"] * (1, -1)[int(pt_cp.vl["LWI_01"]["LWI_VZ_Lenkradwinkel"])] ret.steeringRateDeg = pt_cp.vl["LWI_01"]["LWI_Lenkradw_Geschw"] * (1, -1)[int(pt_cp.vl["LWI_01"]["LWI_VZ_Lenkradw_Geschw"])] ret.steeringTorque = pt_cp.vl["LH_EPS_03"]["EPS_Lenkmoment"] * (1, -1)[int(pt_cp.vl["LH_EPS_03"]["EPS_VZ_Lenkmoment"])] ret.steeringPressed = abs(ret.steeringTorque) > CarControllerParams.STEER_DRIVER_ALLOWANCE @@ -41,8 +41,8 @@ def update(self, pt_cp, cam_cp, ext_cp, trans_type): # Verify EPS readiness to accept steering commands hca_status = self.hca_status_values.get(pt_cp.vl["LH_EPS_03"]["EPS_HCA_Status"]) - ret.steerError = hca_status in ["DISABLED", "FAULT"] - ret.steerWarning = hca_status in ["INITIALIZING", "REJECTED"] + ret.steerError = hca_status in ("DISABLED", "FAULT") + ret.steerWarning = hca_status in ("INITIALIZING", "REJECTED") # Update gas, brakes, and gearshift. ret.gas = pt_cp.vl["Motor_20"]["MO_Fahrpedalrohwert_01"] / 100.0 @@ -102,7 +102,7 @@ def update(self, pt_cp, cam_cp, ext_cp, trans_type): # ACC okay and enabled, but not currently engaged ret.cruiseState.available = True ret.cruiseState.enabled = False - elif self.tsk_status in [3, 4, 5]: + elif self.tsk_status in (3, 4, 5): # ACC okay and enabled, currently regulating speed (3) or driver accel override (4) or overrun coast-down (5) ret.cruiseState.available = True ret.cruiseState.enabled = True @@ -147,50 +147,49 @@ def update(self, pt_cp, cam_cp, ext_cp, trans_type): @staticmethod def get_can_parser(CP): - # this function generates lists for signal, messages and initial values signals = [ - # sig_name, sig_address, default - ("EPS_Berechneter_LW", "LH_EPS_03", 0), # Absolute steering angle - ("EPS_VZ_BLW", "LH_EPS_03", 0), # Steering angle sign - ("LWI_Lenkradw_Geschw", "LWI_01", 0), # Absolute steering rate - ("LWI_VZ_Lenkradw_Geschw", "LWI_01", 0), # Steering rate sign - ("ESP_VL_Radgeschw_02", "ESP_19", 0), # ABS wheel speed, front left - ("ESP_VR_Radgeschw_02", "ESP_19", 0), # ABS wheel speed, front right - ("ESP_HL_Radgeschw_02", "ESP_19", 0), # ABS wheel speed, rear left - ("ESP_HR_Radgeschw_02", "ESP_19", 0), # ABS wheel speed, rear right - ("ESP_Gierrate", "ESP_02", 0), # Absolute yaw rate - ("ESP_VZ_Gierrate", "ESP_02", 0), # Yaw rate sign - ("ZV_FT_offen", "Gateway_72", 0), # Door open, driver - ("ZV_BT_offen", "Gateway_72", 0), # Door open, passenger - ("ZV_HFS_offen", "Gateway_72", 0), # Door open, rear left - ("ZV_HBFS_offen", "Gateway_72", 0), # Door open, rear right - ("ZV_HD_offen", "Gateway_72", 0), # Trunk or hatch open - ("Comfort_Signal_Left", "Blinkmodi_02", 0), # Left turn signal including comfort blink interval - ("Comfort_Signal_Right", "Blinkmodi_02", 0), # Right turn signal including comfort blink interval - ("AB_Gurtschloss_FA", "Airbag_02", 0), # Seatbelt status, driver - ("AB_Gurtschloss_BF", "Airbag_02", 0), # Seatbelt status, passenger - ("ESP_Fahrer_bremst", "ESP_05", 0), # Brake pedal pressed - ("ESP_Bremsdruck", "ESP_05", 0), # Brake pressure applied - ("MO_Fahrpedalrohwert_01", "Motor_20", 0), # Accelerator pedal value - ("EPS_Lenkmoment", "LH_EPS_03", 0), # Absolute driver torque input - ("EPS_VZ_Lenkmoment", "LH_EPS_03", 0), # Driver torque input sign - ("EPS_HCA_Status", "LH_EPS_03", 3), # EPS HCA control status - ("ESP_Tastung_passiv", "ESP_21", 0), # Stability control disabled - ("ESP_Haltebestaetigung", "ESP_21", 0), # ESP hold confirmation - ("KBI_MFA_v_Einheit_02", "Einheiten_01", 0), # MPH vs KMH speed display - ("KBI_Handbremse", "Kombi_01", 0), # Manual handbrake applied - ("TSK_Status", "TSK_06", 0), # ACC engagement status from drivetrain coordinator - ("GRA_Hauptschalter", "GRA_ACC_01", 0), # ACC button, on/off - ("GRA_Abbrechen", "GRA_ACC_01", 0), # ACC button, cancel - ("GRA_Tip_Setzen", "GRA_ACC_01", 0), # ACC button, set - ("GRA_Tip_Hoch", "GRA_ACC_01", 0), # ACC button, increase or accel - ("GRA_Tip_Runter", "GRA_ACC_01", 0), # ACC button, decrease or decel - ("GRA_Tip_Wiederaufnahme", "GRA_ACC_01", 0), # ACC button, resume - ("GRA_Verstellung_Zeitluecke", "GRA_ACC_01", 0), # ACC button, time gap adj - ("GRA_Typ_Hauptschalter", "GRA_ACC_01", 0), # ACC main button type - ("GRA_Tip_Stufe_2", "GRA_ACC_01", 0), # unknown related to stalk type - ("GRA_ButtonTypeInfo", "GRA_ACC_01", 0), # unknown related to stalk type - ("COUNTER", "GRA_ACC_01", 0), # GRA_ACC_01 CAN message counter + # sig_name, sig_address + ("LWI_Lenkradwinkel", "LWI_01"), # Absolute steering angle + ("LWI_VZ_Lenkradwinkel", "LWI_01"), # Steering angle sign + ("LWI_Lenkradw_Geschw", "LWI_01"), # Absolute steering rate + ("LWI_VZ_Lenkradw_Geschw", "LWI_01"), # Steering rate sign + ("ESP_VL_Radgeschw_02", "ESP_19"), # ABS wheel speed, front left + ("ESP_VR_Radgeschw_02", "ESP_19"), # ABS wheel speed, front right + ("ESP_HL_Radgeschw_02", "ESP_19"), # ABS wheel speed, rear left + ("ESP_HR_Radgeschw_02", "ESP_19"), # ABS wheel speed, rear right + ("ESP_Gierrate", "ESP_02"), # Absolute yaw rate + ("ESP_VZ_Gierrate", "ESP_02"), # Yaw rate sign + ("ZV_FT_offen", "Gateway_72"), # Door open, driver + ("ZV_BT_offen", "Gateway_72"), # Door open, passenger + ("ZV_HFS_offen", "Gateway_72"), # Door open, rear left + ("ZV_HBFS_offen", "Gateway_72"), # Door open, rear right + ("ZV_HD_offen", "Gateway_72"), # Trunk or hatch open + ("Comfort_Signal_Left", "Blinkmodi_02"), # Left turn signal including comfort blink interval + ("Comfort_Signal_Right", "Blinkmodi_02"), # Right turn signal including comfort blink interval + ("AB_Gurtschloss_FA", "Airbag_02"), # Seatbelt status, driver + ("AB_Gurtschloss_BF", "Airbag_02"), # Seatbelt status, passenger + ("ESP_Fahrer_bremst", "ESP_05"), # Brake pedal pressed + ("ESP_Bremsdruck", "ESP_05"), # Brake pressure applied + ("MO_Fahrpedalrohwert_01", "Motor_20"), # Accelerator pedal value + ("EPS_Lenkmoment", "LH_EPS_03"), # Absolute driver torque input + ("EPS_VZ_Lenkmoment", "LH_EPS_03"), # Driver torque input sign + ("EPS_HCA_Status", "LH_EPS_03"), # EPS HCA control status + ("ESP_Tastung_passiv", "ESP_21"), # Stability control disabled + ("ESP_Haltebestaetigung", "ESP_21"), # ESP hold confirmation + ("KBI_MFA_v_Einheit_02", "Einheiten_01"), # MPH vs KMH speed display + ("KBI_Handbremse", "Kombi_01"), # Manual handbrake applied + ("TSK_Status", "TSK_06"), # ACC engagement status from drivetrain coordinator + ("GRA_Hauptschalter", "GRA_ACC_01"), # ACC button, on/off + ("GRA_Abbrechen", "GRA_ACC_01"), # ACC button, cancel + ("GRA_Tip_Setzen", "GRA_ACC_01"), # ACC button, set + ("GRA_Tip_Hoch", "GRA_ACC_01"), # ACC button, increase or accel + ("GRA_Tip_Runter", "GRA_ACC_01"), # ACC button, decrease or decel + ("GRA_Tip_Wiederaufnahme", "GRA_ACC_01"), # ACC button, resume + ("GRA_Verstellung_Zeitluecke", "GRA_ACC_01"), # ACC button, time gap adj + ("GRA_Typ_Hauptschalter", "GRA_ACC_01"), # ACC main button type + ("GRA_Tip_Stufe_2", "GRA_ACC_01"), # unknown related to stalk type + ("GRA_ButtonTypeInfo", "GRA_ACC_01"), # unknown related to stalk type + ("COUNTER", "GRA_ACC_01"), # GRA_ACC_01 CAN message counter ] checks = [ @@ -212,15 +211,15 @@ def get_can_parser(CP): ] if CP.transmissionType == TransmissionType.automatic: - signals += [("GE_Fahrstufe", "Getriebe_11", 0)] # Auto trans gear selector position - checks += [("Getriebe_11", 20)] # From J743 Auto transmission control module + signals.append(("GE_Fahrstufe", "Getriebe_11")) # Auto trans gear selector position + checks.append(("Getriebe_11", 20)) # From J743 Auto transmission control module elif CP.transmissionType == TransmissionType.direct: - signals += [("GearPosition", "EV_Gearshift", 0)] # EV gear selector position - checks += [("EV_Gearshift", 10)] # From J??? unknown EV control module + signals.append(("GearPosition", "EV_Gearshift")) # EV gear selector position + checks.append(("EV_Gearshift", 10)) # From J??? unknown EV control module elif CP.transmissionType == TransmissionType.manual: - signals += [("MO_Kuppl_schalter", "Motor_14", 0), # Clutch switch - ("BCM1_Rueckfahrlicht_Schalter", "Gateway_72", 0)] # Reverse light from BCM - checks += [("Motor_14", 10)] # From J623 Engine control module + signals += [("MO_Kuppl_schalter", "Motor_14"), # Clutch switch + ("BCM1_Rueckfahrlicht_Schalter", "Gateway_72")] # Reverse light from BCM + checks.append(("Motor_14", 10)) # From J623 Engine control module if CP.networkLocation == NetworkLocation.fwdCamera: # Radars are here on CANBUS.pt @@ -234,18 +233,17 @@ def get_can_parser(CP): @staticmethod def get_cam_can_parser(CP): - signals = [] checks = [] if CP.networkLocation == NetworkLocation.fwdCamera: signals += [ - # sig_name, sig_address, default - ("LDW_SW_Warnung_links", "LDW_02", 0), # Blind spot in warning mode on left side due to lane departure - ("LDW_SW_Warnung_rechts", "LDW_02", 0), # Blind spot in warning mode on right side due to lane departure - ("LDW_Seite_DLCTLC", "LDW_02", 0), # Direction of most likely lane departure (left or right) - ("LDW_DLC", "LDW_02", 0), # Lane departure, distance to line crossing - ("LDW_TLC", "LDW_02", 0), # Lane departure, time to line crossing + # sig_name, sig_address + ("LDW_SW_Warnung_links", "LDW_02"), # Blind spot in warning mode on left side due to lane departure + ("LDW_SW_Warnung_rechts", "LDW_02"), # Blind spot in warning mode on right side due to lane departure + ("LDW_Seite_DLCTLC", "LDW_02"), # Direction of most likely lane departure (left or right) + ("LDW_DLC", "LDW_02"), # Lane departure, distance to line crossing + ("LDW_TLC", "LDW_02"), # Lane departure, time to line crossing ] checks += [ # sig_address, frequency @@ -264,20 +262,20 @@ def get_cam_can_parser(CP): class MqbExtraSignals: # Additional signal and message lists for optional or bus-portable controllers fwd_radar_signals = [ - ("ACC_Wunschgeschw", "ACC_02", 0), # ACC set speed - ("AWV2_Freigabe", "ACC_10", 0), # FCW brake jerk release - ("ANB_Teilbremsung_Freigabe", "ACC_10", 0), # AEB partial braking release - ("ANB_Zielbremsung_Freigabe", "ACC_10", 0), # AEB target braking release + ("ACC_Wunschgeschw", "ACC_02"), # ACC set speed + ("AWV2_Freigabe", "ACC_10"), # FCW brake jerk release + ("ANB_Teilbremsung_Freigabe", "ACC_10"), # AEB partial braking release + ("ANB_Zielbremsung_Freigabe", "ACC_10"), # AEB target braking release ] fwd_radar_checks = [ ("ACC_10", 50), # From J428 ACC radar control module ("ACC_02", 17), # From J428 ACC radar control module ] bsm_radar_signals = [ - ("SWA_Infostufe_SWA_li", "SWA_01", 0), # Blind spot object info, left - ("SWA_Warnung_SWA_li", "SWA_01", 0), # Blind spot object warning, left - ("SWA_Infostufe_SWA_re", "SWA_01", 0), # Blind spot object info, right - ("SWA_Warnung_SWA_re", "SWA_01", 0), # Blind spot object warning, right + ("SWA_Infostufe_SWA_li", "SWA_01"), # Blind spot object info, left + ("SWA_Warnung_SWA_li", "SWA_01"), # Blind spot object warning, left + ("SWA_Infostufe_SWA_re", "SWA_01"), # Blind spot object info, right + ("SWA_Warnung_SWA_re", "SWA_01"), # Blind spot object warning, right ] bsm_radar_checks = [ ("SWA_01", 20), # From J1086 Lane Change Assist diff --git a/selfdrive/car/volkswagen/interface.py b/selfdrive/car/volkswagen/interface.py index 84bfe987716ca3..961cfed7fe1d1a 100644 --- a/selfdrive/car/volkswagen/interface.py +++ b/selfdrive/car/volkswagen/interface.py @@ -38,14 +38,14 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None): else: ret.transmissionType = TransmissionType.manual - if any(msg in fingerprint[1] for msg in [0x40, 0x86, 0xB2, 0xFD]): # Airbag_01, LWI_01, ESP_19, ESP_21 + if any(msg in fingerprint[1] for msg in (0x40, 0x86, 0xB2, 0xFD)): # Airbag_01, LWI_01, ESP_19, ESP_21 ret.networkLocation = NetworkLocation.gateway else: ret.networkLocation = NetworkLocation.fwdCamera # Global lateral tuning defaults, can be overridden per-vehicle - ret.steerActuatorDelay = 0.05 + ret.steerActuatorDelay = 0.1 ret.steerRateCost = 1.0 ret.steerLimitTimer = 0.4 ret.steerRatio = 15.6 # Let the params learner figure this out @@ -174,12 +174,6 @@ def update(self, c, can_strings): ret.canValid = self.cp.can_valid and self.cp_cam.can_valid ret.steeringRateLimited = self.CC.steer_rate_limited if self.CC is not None else False - # TODO: add a field for this to carState, car interface code shouldn't write params - # Update the device metric configuration to match the car at first startup, - # or if there's been a change. - #if self.CS.displayMetricUnits != self.displayMetricUnitsPrev: - # put_nonblocking("IsMetric", "1" if self.CS.displayMetricUnits else "0") - # Check for and process state-change events (button press or release) from # the turn stalk switch or ACC steering wheel/control stalk buttons. for button in self.CS.buttonStates: @@ -194,7 +188,7 @@ def update(self, c, can_strings): # Vehicle health and operation safety checks if self.CS.parkingBrakeSet: events.add(EventName.parkBrake) - if self.CS.tsk_status in [6, 7]: + if self.CS.tsk_status in (6, 7): events.add(EventName.accFaulted) # Low speed steer alert hysteresis logic @@ -217,7 +211,7 @@ def update(self, c, can_strings): def apply(self, c): hud_control = c.hudControl - ret = self.CC.update(c.enabled, self.CS, self.frame, self.ext_bus, c.actuators, + ret = self.CC.update(c, c.enabled, self.CS, self.frame, self.ext_bus, c.actuators, hud_control.visualAlert, hud_control.leftLaneVisible, hud_control.rightLaneVisible, diff --git a/selfdrive/car/volkswagen/values.py b/selfdrive/car/volkswagen/values.py index d937ecad974829..57404fe5a7a617 100755 --- a/selfdrive/car/volkswagen/values.py +++ b/selfdrive/car/volkswagen/values.py @@ -1,5 +1,3 @@ -# flake8: noqa - from collections import defaultdict from typing import Dict @@ -124,6 +122,7 @@ class CAR: CAR.ATLAS_MK1: { (Ecu.engine, 0x7e0, None): [ b'\xf1\x8703H906026AA\xf1\x899970', + b'\xf1\x8703H906026AT\xf1\x891922', b'\xf1\x8703H906026F \xf1\x896696', b'\xf1\x8703H906026F \xf1\x899970', b'\xf1\x8703H906026J \xf1\x896026', @@ -134,17 +133,21 @@ class CAR: (Ecu.transmission, 0x7e1, None): [ b'\xf1\x8709G927158A \xf1\x893387', b'\xf1\x8709G927158DR\xf1\x893536', + b'\xf1\x8709G927158FT\xf1\x893835', ], (Ecu.srs, 0x715, None): [ b'\xf1\x873Q0959655BC\xf1\x890503\xf1\x82\0161914151912001103111122031200', b'\xf1\x873Q0959655BN\xf1\x890713\xf1\x82\0162214152212001105141122052900', b'\xf1\x873Q0959655DB\xf1\x890720\xf1\x82\0162214152212001105141122052900', + b'\xf1\x873Q0959655DM\xf1\x890732\xf1\x82\x0e1114151112001105161122052J00', ], (Ecu.eps, 0x712, None): [ b'\xf1\x873QF909144B \xf1\x891582\xf1\x82\00571B60924A1', + b'\xf1\x873QF909144B \xf1\x891582\xf1\x82\x0571B6G920A1', b'\xf1\x875Q0909143P \xf1\x892051\xf1\x820528B6090105', ], (Ecu.fwdRadar, 0x757, None): [ + b'\xf1\x872Q0907572T \xf1\x890383', b'\xf1\x875Q0907572H \xf1\x890620', b'\xf1\x875Q0907572J \xf1\x890654', b'\xf1\x875Q0907572P \xf1\x890682', @@ -162,6 +165,7 @@ class CAR: b'\xf1\x8704E906027HD\xf1\x893742', b'\xf1\x8704E906027MA\xf1\x894958', b'\xf1\x8704L906021DT\xf1\x895520', + b'\xf1\x8704L906021DT\xf1\x898127', b'\xf1\x8704L906021N \xf1\x895518', b'\xf1\x8704L906026BP\xf1\x897608', b'\xf1\x8704L906026NF\xf1\x899528', @@ -208,6 +212,7 @@ class CAR: b'\xf1\x870D9300040A \xf1\x893613', b'\xf1\x870D9300040S \xf1\x894311', b'\xf1\x870D9300041H \xf1\x895220', + b'\xf1\x870D9300041P \xf1\x894507', b'\xf1\x870DD300045K \xf1\x891120', b'\xf1\x870DD300046F \xf1\x891601', b'\xf1\x870GC300012A \xf1\x891403', diff --git a/selfdrive/common/SConscript b/selfdrive/common/SConscript index 9b496fef72d872..8dfeecbdd7b4b3 100644 --- a/selfdrive/common/SConscript +++ b/selfdrive/common/SConscript @@ -7,6 +7,7 @@ else: common_libs = [ 'params.cc', + 'statlog.cc', 'swaglog.cc', 'util.cc', 'gpio.cc', @@ -33,3 +34,4 @@ Export('_common', '_gpucommon', '_gpu_libs') if GetOption('test'): env.Program('tests/test_util', ['tests/test_util.cc'], LIBS=[_common]) + env.Program('tests/test_swaglog', ['tests/test_swaglog.cc'], LIBS=[_common, 'json11', 'zmq', 'pthread']) diff --git a/selfdrive/common/params.cc b/selfdrive/common/params.cc index 987fc634d7048b..4f1266bbaa37f8 100644 --- a/selfdrive/common/params.cc +++ b/selfdrive/common/params.cc @@ -86,15 +86,13 @@ std::unordered_map keys = { {"AccessToken", CLEAR_ON_MANAGER_START | DONT_LOG}, {"AthenadPid", PERSISTENT}, {"AthenadUploadQueue", PERSISTENT}, - {"BootedOnroad", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_OFF}, {"CalibrationParams", PERSISTENT}, {"CarBatteryCapacity", PERSISTENT}, - {"CarParams", CLEAR_ON_MANAGER_START | CLEAR_ON_PANDA_DISCONNECT | CLEAR_ON_IGNITION_ON}, - {"CarParamsCache", PERSISTENT}, - {"CarVin", CLEAR_ON_MANAGER_START | CLEAR_ON_PANDA_DISCONNECT | CLEAR_ON_IGNITION_ON}, - {"CommunityFeaturesToggle", PERSISTENT}, + {"CarParams", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_ON}, + {"CarParamsCache", CLEAR_ON_MANAGER_START}, + {"CarVin", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_ON}, {"CompletedTrainingVersion", PERSISTENT}, - {"ControlsReady", CLEAR_ON_MANAGER_START | CLEAR_ON_PANDA_DISCONNECT | CLEAR_ON_IGNITION_ON}, + {"ControlsReady", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_ON}, {"CurrentRoute", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_ON}, {"DisablePowerDown", PERSISTENT}, {"DisableRadar_Allow", PERSISTENT}, @@ -121,6 +119,7 @@ std::unordered_map keys = { {"IMEI", PERSISTENT}, {"InstallDate", PERSISTENT}, {"IsDriverViewEnabled", CLEAR_ON_MANAGER_START}, + {"IsEngaged", PERSISTENT}, {"IsLdwEnabled", PERSISTENT}, {"IsMetric", PERSISTENT}, {"IsOffroad", CLEAR_ON_MANAGER_START}, @@ -131,7 +130,10 @@ std::unordered_map keys = { {"JoystickDebugMode", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_OFF}, {"LastAthenaPingTime", CLEAR_ON_MANAGER_START}, {"LastGPSPosition", PERSISTENT}, + {"LastManagerExitReason", CLEAR_ON_MANAGER_START}, + {"LastPeripheralPandaType", PERSISTENT}, {"LastPowerDropDetected", CLEAR_ON_MANAGER_START}, + {"LastSystemShutdown", CLEAR_ON_MANAGER_START}, {"LastUpdateException", PERSISTENT}, {"LastUpdateTime", PERSISTENT}, {"LiveParameters", PERSISTENT}, @@ -140,6 +142,7 @@ std::unordered_map keys = { {"NavdRender", PERSISTENT}, {"OpenpilotEnabledToggle", PERSISTENT}, {"PandaHeartbeatLost", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_OFF}, + {"PandaSignatures", CLEAR_ON_MANAGER_START}, {"Passive", PERSISTENT}, {"PrimeAdDismissed", PERSISTENT}, {"PrimeRedirected", PERSISTENT}, @@ -163,7 +166,7 @@ std::unordered_map keys = { {"ApiCache_NavDestinations", PERSISTENT}, {"ApiCache_Owner", PERSISTENT}, {"Offroad_CarUnrecognized", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_ON}, - {"Offroad_ChargeDisabled", CLEAR_ON_MANAGER_START | CLEAR_ON_PANDA_DISCONNECT}, + {"Offroad_ChargeDisabled", CLEAR_ON_MANAGER_START }, {"Offroad_ConnectivityNeeded", CLEAR_ON_MANAGER_START}, {"Offroad_ConnectivityNeededPrompt", CLEAR_ON_MANAGER_START}, {"Offroad_InvalidTime", CLEAR_ON_MANAGER_START}, diff --git a/selfdrive/common/params.h b/selfdrive/common/params.h index c4bdde0012f1a8..be09c63d54094e 100644 --- a/selfdrive/common/params.h +++ b/selfdrive/common/params.h @@ -6,10 +6,9 @@ enum ParamKeyType { PERSISTENT = 0x02, CLEAR_ON_MANAGER_START = 0x04, - CLEAR_ON_PANDA_DISCONNECT = 0x08, - CLEAR_ON_IGNITION_ON = 0x10, - CLEAR_ON_IGNITION_OFF = 0x20, - DONT_LOG = 0x40, + CLEAR_ON_IGNITION_ON = 0x08, + CLEAR_ON_IGNITION_OFF = 0x10, + DONT_LOG = 0x20, ALL = 0xFFFFFFFF }; diff --git a/selfdrive/common/statlog.cc b/selfdrive/common/statlog.cc new file mode 100644 index 00000000000000..27dfc2ca9d10b7 --- /dev/null +++ b/selfdrive/common/statlog.cc @@ -0,0 +1,43 @@ +#ifndef _GNU_SOURCE +#define _GNU_SOURCE +#endif + +#include "selfdrive/common/statlog.h" +#include "selfdrive/common/util.h" + +#include +#include +#include + +class StatlogState : public LogState { + public: + StatlogState() : LogState("ipc:///tmp/stats") {} +}; + +static StatlogState s = {}; + +static void log(const char* metric_type, const char* metric, const char* fmt, ...) { + char* value_buf = nullptr; + va_list args; + va_start(args, fmt); + int ret = vasprintf(&value_buf, fmt, args); + va_end(args); + + if (ret > 0 && value_buf) { + char* line_buf = nullptr; + ret = asprintf(&line_buf, "%s:%s|%s", metric, value_buf, metric_type); + if (ret > 0 && line_buf) { + zmq_send(s.sock, line_buf, ret, ZMQ_NOBLOCK); + free(line_buf); + } + free(value_buf); + } +} + +void statlog_log(const char* metric_type, const char* metric, int value) { + log(metric_type, metric, "%d", value); +} + +void statlog_log(const char* metric_type, const char* metric, float value) { + log(metric_type, metric, "%f", value); +} diff --git a/selfdrive/common/statlog.h b/selfdrive/common/statlog.h new file mode 100644 index 00000000000000..5d223bb666dc75 --- /dev/null +++ b/selfdrive/common/statlog.h @@ -0,0 +1,10 @@ +#pragma once + +#define STATLOG_GAUGE "g" +#define STATLOG_SAMPLE "sa" + +void statlog_log(const char* metric_type, const char* metric, int value); +void statlog_log(const char* metric_type, const char* metric, float value); + +#define statlog_gauge(metric, value) statlog_log(STATLOG_GAUGE, metric, value) +#define statlog_sample(metric, value) statlog_log(STATLOG_SAMPLE, metric, value) diff --git a/selfdrive/common/swaglog.cc b/selfdrive/common/swaglog.cc index 74488e2201e153..1fe700415df74d 100644 --- a/selfdrive/common/swaglog.cc +++ b/selfdrive/common/swaglog.cc @@ -16,75 +16,55 @@ #include "selfdrive/common/version.h" #include "selfdrive/hardware/hw.h" -class LogState { +class SwaglogState : public LogState { public: - LogState() = default; - ~LogState(); - std::mutex lock; - bool inited; - json11::Json::object ctx_j; - void *zctx; - void *sock; - int print_level; -}; + SwaglogState() : LogState("ipc:///tmp/logmessage") {} -LogState::~LogState() { - zmq_close(sock); - zmq_ctx_destroy(zctx); -} - -static LogState s = {}; + bool initialized = false; + json11::Json::object ctx_j; -static void cloudlog_bind_locked(const char* k, const char* v) { - s.ctx_j[k] = v; -} + inline void initialize() { + ctx_j = json11::Json::object {}; + print_level = CLOUDLOG_WARNING; + const char* print_lvl = getenv("LOGPRINT"); + if (print_lvl) { + if (strcmp(print_lvl, "debug") == 0) { + print_level = CLOUDLOG_DEBUG; + } else if (strcmp(print_lvl, "info") == 0) { + print_level = CLOUDLOG_INFO; + } else if (strcmp(print_lvl, "warning") == 0) { + print_level = CLOUDLOG_WARNING; + } + } -static void cloudlog_init() { - if (s.inited) return; - s.ctx_j = json11::Json::object {}; - s.zctx = zmq_ctx_new(); - s.sock = zmq_socket(s.zctx, ZMQ_PUSH); - - int timeout = 100; // 100 ms timeout on shutdown for messages to be received by logmessaged - zmq_setsockopt(s.sock, ZMQ_LINGER, &timeout, sizeof(timeout)); - - zmq_connect(s.sock, "ipc:///tmp/logmessage"); - - s.print_level = CLOUDLOG_WARNING; - const char* print_level = getenv("LOGPRINT"); - if (print_level) { - if (strcmp(print_level, "debug") == 0) { - s.print_level = CLOUDLOG_DEBUG; - } else if (strcmp(print_level, "info") == 0) { - s.print_level = CLOUDLOG_INFO; - } else if (strcmp(print_level, "warning") == 0) { - s.print_level = CLOUDLOG_WARNING; + // openpilot bindings + char* dongle_id = getenv("DONGLE_ID"); + if (dongle_id) { + ctx_j["dongle_id"] = dongle_id; + } + char* daemon_name = getenv("MANAGER_DAEMON"); + if (daemon_name) { + ctx_j["daemon"] = daemon_name; + } + ctx_j["version"] = COMMA_VERSION; + ctx_j["dirty"] = !getenv("CLEAN"); + + // device type + if (Hardware::EON()) { + ctx_j["device"] = "eon"; + } else if (Hardware::TICI()) { + ctx_j["device"] = "tici"; + } else { + ctx_j["device"] = "pc"; } - } - // openpilot bindings - char* dongle_id = getenv("DONGLE_ID"); - if (dongle_id) { - cloudlog_bind_locked("dongle_id", dongle_id); - } - cloudlog_bind_locked("version", COMMA_VERSION); - s.ctx_j["dirty"] = !getenv("CLEAN"); - - // device type - if (Hardware::EON()) { - cloudlog_bind_locked("device", "eon"); - } else if (Hardware::TICI()) { - cloudlog_bind_locked("device", "tici"); - } else { - cloudlog_bind_locked("device", "pc"); + initialized = true; } +}; - s.inited = true; -} +static SwaglogState s = {}; -void log(int levelnum, const char* filename, int lineno, const char* func, const char* msg, const std::string& log_s) { - std::lock_guard lk(s.lock); - cloudlog_init(); +static void log(int levelnum, const char* filename, int lineno, const char* func, const char* msg, const std::string& log_s) { if (levelnum >= s.print_level) { printf("%s: %s\n", filename, msg); } @@ -97,10 +77,14 @@ void cloudlog_e(int levelnum, const char* filename, int lineno, const char* func char* msg_buf = nullptr; va_list args; va_start(args, fmt); - vasprintf(&msg_buf, fmt, args); + int ret = vasprintf(&msg_buf, fmt, args); va_end(args); - if (!msg_buf) return; + if (ret <= 0 || !msg_buf) return; + + std::lock_guard lk(s.lock); + + if (!s.initialized) s.initialize(); json11::Json log_j = json11::Json::object { {"msg", msg_buf}, @@ -115,9 +99,3 @@ void cloudlog_e(int levelnum, const char* filename, int lineno, const char* func log(levelnum, filename, lineno, func, msg_buf, log_s); free(msg_buf); } - -void cloudlog_bind(const char* k, const char* v) { - std::lock_guard lk(s.lock); - cloudlog_init(); - cloudlog_bind_locked(k, v); -} diff --git a/selfdrive/common/swaglog.h b/selfdrive/common/swaglog.h index 9a1d3c0a6758a1..6403820ac83a85 100644 --- a/selfdrive/common/swaglog.h +++ b/selfdrive/common/swaglog.h @@ -8,11 +8,11 @@ #define CLOUDLOG_ERROR 40 #define CLOUDLOG_CRITICAL 50 + + void cloudlog_e(int levelnum, const char* filename, int lineno, const char* func, const char* fmt, ...) /*__attribute__ ((format (printf, 6, 7)))*/; -void cloudlog_bind(const char* k, const char* v); - #define cloudlog(lvl, fmt, ...) cloudlog_e(lvl, __FILE__, __LINE__, \ __func__, \ fmt, ## __VA_ARGS__) diff --git a/selfdrive/common/tests/.gitignore b/selfdrive/common/tests/.gitignore index 2b7a3c6eb89705..1350b3b825c050 100644 --- a/selfdrive/common/tests/.gitignore +++ b/selfdrive/common/tests/.gitignore @@ -1 +1,2 @@ test_util +test_swaglog diff --git a/selfdrive/common/tests/test_swaglog.cc b/selfdrive/common/tests/test_swaglog.cc new file mode 100644 index 00000000000000..1d00def63d02e5 --- /dev/null +++ b/selfdrive/common/tests/test_swaglog.cc @@ -0,0 +1,94 @@ +#include +#include +#define CATCH_CONFIG_MAIN +#include "catch2/catch.hpp" + +#include "json11.hpp" +#include "selfdrive/common/swaglog.h" +#include "selfdrive/common/util.h" +#include "selfdrive/common/version.h" +#include "selfdrive/hardware/hw.h" + +const char *SWAGLOG_ADDR = "ipc:///tmp/logmessage"; +std::string daemon_name = "testy"; +std::string dongle_id = "test_dongle_id"; +int LINE_NO = 0; + +void log_thread(int thread_id, int msg_cnt) { + for (int i = 0; i < msg_cnt; ++i) { + LOGD("%d", thread_id); + LINE_NO = __LINE__ - 1; + usleep(1); + } +} + +void recv_log(int thread_cnt, int thread_msg_cnt) { + void *zctx = zmq_ctx_new(); + void *sock = zmq_socket(zctx, ZMQ_PULL); + zmq_bind(sock, SWAGLOG_ADDR); + std::vector thread_msgs(thread_cnt); + int total_count = 0; + + for (auto start = std::chrono::steady_clock::now(), now = start; + now < start + std::chrono::seconds{1} && total_count < (thread_cnt * thread_msg_cnt); + now = std::chrono::steady_clock::now()) { + char buf[4096] = {}; + if (zmq_recv(sock, buf, sizeof(buf), ZMQ_DONTWAIT) <= 0) { + if (errno == EAGAIN || errno == EINTR || errno == EFSM) continue; + break; + } + + REQUIRE(buf[0] == CLOUDLOG_DEBUG); + std::string err; + auto msg = json11::Json::parse(buf + 1, err); + REQUIRE(!msg.is_null()); + + REQUIRE(msg["levelnum"].int_value() == CLOUDLOG_DEBUG); + REQUIRE_THAT(msg["filename"].string_value(), Catch::Contains("test_swaglog.cc")); + REQUIRE(msg["funcname"].string_value() == "log_thread"); + REQUIRE(msg["lineno"].int_value() == LINE_NO); + + auto ctx = msg["ctx"]; + + REQUIRE(ctx["daemon"].string_value() == daemon_name); + REQUIRE(ctx["dongle_id"].string_value() == dongle_id); + REQUIRE(ctx["dirty"].bool_value() == true); + + REQUIRE(ctx["version"].string_value() == COMMA_VERSION); + + std::string device = "pc"; + if (Hardware::EON()) { + device = "eon"; + } else if (Hardware::TICI()) { + device = "tici"; + } + REQUIRE(ctx["device"].string_value() == device); + + int thread_id = atoi(msg["msg"].string_value().c_str()); + REQUIRE((thread_id >= 0 && thread_id < thread_cnt)); + thread_msgs[thread_id]++; + total_count++; + } + for (int i = 0; i < thread_cnt; ++i) { + INFO("thread :" << i); + REQUIRE(thread_msgs[i] == thread_msg_cnt); + } + zmq_close(sock); + zmq_ctx_destroy(zctx); +} + +TEST_CASE("swaglog") { + setenv("MANAGER_DAEMON", daemon_name.c_str(), 1); + setenv("DONGLE_ID", dongle_id.c_str(), 1); + setenv("dirty", "1", 1); + const int thread_cnt = 5; + const int thread_msg_cnt = 100; + + std::vector log_threads; + for (int i = 0; i < thread_cnt; ++i) { + log_threads.push_back(std::thread(log_thread, i, thread_msg_cnt)); + } + for (auto &t : log_threads) t.join(); + + recv_log(thread_cnt, thread_msg_cnt); +} diff --git a/selfdrive/common/util.h b/selfdrive/common/util.h index 9a6a4d9bdb72ab..bf0df3bcaa6889 100644 --- a/selfdrive/common/util.h +++ b/selfdrive/common/util.h @@ -3,6 +3,7 @@ #include #include #include +#include #include #include @@ -11,6 +12,7 @@ #include #include #include +#include #include #include #include @@ -91,7 +93,9 @@ bool create_directories(const std::string &dir, mode_t mode); std::string check_output(const std::string& command); inline void sleep_for(const int milliseconds) { - std::this_thread::sleep_for(std::chrono::milliseconds(milliseconds)); + if (milliseconds > 0) { + std::this_thread::sleep_for(std::chrono::milliseconds(milliseconds)); + } } } // namespace util @@ -161,3 +165,26 @@ void update_max_atomic(std::atomic& max, T const& value) { T prev = max; while(prev < value && !max.compare_exchange_weak(prev, value)) {} } + +class LogState { + public: + std::mutex lock; + void *zctx; + void *sock; + int print_level; + + LogState(const char* endpoint) { + zctx = zmq_ctx_new(); + sock = zmq_socket(zctx, ZMQ_PUSH); + + // Timeout on shutdown for messages to be received by the logging process + int timeout = 100; + zmq_setsockopt(sock, ZMQ_LINGER, &timeout, sizeof(timeout)); + + zmq_connect(sock, endpoint); + }; + ~LogState() { + zmq_close(sock); + zmq_ctx_destroy(zctx); + } +}; diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 788f3fe26a5b8d..8680a814ad6217 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -10,11 +10,11 @@ from common.realtime import sec_since_boot, config_realtime_process, Priority, Ratekeeper, DT_CTRL from common.profiler import Profiler from common.params import Params, put_nonblocking -from common.travis_checker import gh_actions import cereal.messaging as messaging -import selfdrive.crash as crash +# from common.travis_checker import gh_actions +# import selfdrive.crash as crash +# from selfdrive.version import is_fork_remote from selfdrive.config import Conversions as CV -from selfdrive.version import is_fork_remote from selfdrive.swaglog import cloudlog from selfdrive.boardd.boardd import can_list_to_can_capnp from selfdrive.car.car_helpers import get_car, get_startup_event, get_one_can @@ -38,14 +38,13 @@ SOFT_DISABLE_TIME = 3 # seconds LDW_MIN_SPEED = 31 * CV.MPH_TO_MS LANE_DEPARTURE_THRESHOLD = 0.1 -STEER_ANGLE_SATURATION_TIMEOUT = 1.0 / DT_CTRL -STEER_ANGLE_SATURATION_THRESHOLD = 2.5 # Degrees REPLAY = "REPLAY" in os.environ SIMULATION = "SIMULATION" in os.environ NOSENSOR = "NOSENSOR" in os.environ IGNORE_PROCESSES = {"rtshield", "uploader", "deleter", "loggerd", "logmessaged", "tombstoned", - "logcatd", "proclogd", "clocksd", "updated", "timezoned", "manage_athenad"} | \ + "logcatd", "proclogd", "clocksd", "updated", "timezoned", "manage_athenad", + "statsd", "shutdownd"} | \ {k for k, v in managed_processes.items() if not v.enabled} ACTUATOR_FIELDS = set(car.CarControl.Actuators.schema.fields.keys()) @@ -61,17 +60,17 @@ SafetyModel = car.CarParams.SafetyModel IGNORED_SAFETY_MODES = [SafetyModel.silent, SafetyModel.noOutput] +CSID_MAP = {"0": EventName.roadCameraError, "1": EventName.wideRoadCameraError, "2": EventName.driverCameraError} - -def log_fingerprint(candidate, timeout=15): - if not gh_actions and is_fork_remote: - try: - requests.get('https://sentry.io', timeout=timeout) - crash.init() - crash.capture_message("fingerprinted {}".format(candidate), level='info') - return - except: - pass +# def log_fingerprint(candidate, timeout=15): +# if not gh_actions and is_fork_remote: +# try: +# requests.get('https://sentry.io', timeout=timeout) +# crash.init() +# crash.capture_message("fingerprinted {}".format(candidate), level='info') +# return +# except: +# pass class Controls: @@ -121,12 +120,11 @@ def __init__(self, sm=None, pm=None, can_sock=None): get_one_can(self.can_sock) self.CI, self.CP, candidate = get_car(self.can_sock, self.pm.sock['sendcan']) - threading.Thread(target=log_fingerprint, args=[candidate]).start() + # threading.Thread(target=log_fingerprint, args=[candidate]).start() # read params self.is_metric = params.get_bool("IsMetric") self.is_ldw_enabled = params.get_bool("IsLdwEnabled") - community_feature_toggle = params.get_bool("CommunityFeaturesToggle") openpilot_enabled_toggle = params.get_bool("OpenpilotEnabledToggle") passive = params.get_bool("Passive") or not openpilot_enabled_toggle @@ -136,11 +134,7 @@ def __init__(self, sm=None, pm=None, can_sock=None): car_recognized = self.CP.carName != 'mock' controller_available = self.CI.CC is not None and not passive and not self.CP.dashcamOnly - community_feature = self.CP.communityFeature or \ - self.CP.fingerprintSource == car.CarParams.FingerprintSource.can - community_feature_disallowed = community_feature and (not community_feature_toggle) - self.read_only = not car_recognized or not controller_available or \ - self.CP.dashcamOnly or community_feature_disallowed + self.read_only = not car_recognized or not controller_available or self.CP.dashcamOnly if self.read_only: safety_config = car.CarParams.SafetyConfig.new_message() safety_config.safetyModel = car.CarParams.SafetyModel.noOutput @@ -159,15 +153,15 @@ def __init__(self, sm=None, pm=None, can_sock=None): self.VM = VehicleModel(self.CP) if self.CP.steerControlType == car.CarParams.SteerControlType.angle: - self.LaC = LatControlAngle(self.CP) + self.LaC = LatControlAngle(self.CP, self.CI) elif self.CP.lateralTuning.which() == 'pid': self.LaC = LatControlPID(self.CP, self.CI) elif self.CP.lateralTuning.which() == 'indi': - self.LaC = LatControlINDI(self.CP) + self.LaC = LatControlINDI(self.CP, self.CI) elif self.CP.lateralTuning.which() == 'lqr': - self.LaC = LatControlLQR(self.CP) + self.LaC = LatControlLQR(self.CP, self.CI) elif self.CP.lateralTuning.which() == 'model': - self.LaC = LatControlModel(self.CP) + self.LaC = LatControlModel(self.CP, self.CI) self.initialized = False self.state = State.disabled @@ -181,7 +175,6 @@ def __init__(self, sm=None, pm=None, can_sock=None): self.cruise_mismatch_counter = 0 self.can_rcv_error_counter = 0 self.last_blinker_frame = 0 - self.saturated_count = 0 self.distance_traveled = 0 self.last_functional_fan_frame = 0 self.events_prev = [] @@ -198,8 +191,6 @@ def __init__(self, sm=None, pm=None, can_sock=None): if not sounds_available: self.events.add(EventName.soundsUnavailable, static=True) - if community_feature_disallowed and car_recognized and not self.CP.dashcamOnly: - self.events.add(EventName.communityFeatureDisallowed, static=True) if not car_recognized: self.events.add(EventName.carUnrecognized, static=True) if len(self.CP.carFw) > 0: @@ -220,10 +211,8 @@ def update_events(self, CS): """Compute carEvents from carState""" self.events.clear() - self.events.add_from_msg(CS.events) - self.events.add_from_msg(self.sm['driverMonitoringState'].events) - # Handle startup event + # Add startup event if self.startup_event is not None: self.events.add(self.startup_event) self.startup_event = None @@ -233,6 +222,9 @@ def update_events(self, CS): self.events.add(EventName.controlsInitializing) return + self.events.add_from_msg(CS.events) + self.events.add_from_msg(self.sm['driverMonitoringState'].events) + # Create events for battery, temperature, disk space, and memory if EON and (self.sm['peripheralState'].pandaType != PandaType.uno) and \ self.sm['deviceState'].batteryPercent < 1 and self.sm['deviceState'].chargingError: @@ -253,7 +245,7 @@ def update_events(self, CS): # self.events.add(EventName.highCpuUsage) # Alert if fan isn't spinning for 5 seconds - if self.sm['peripheralState'].pandaType in [PandaType.uno, PandaType.dos]: + if self.sm['peripheralState'].pandaType in (PandaType.uno, PandaType.dos): if self.sm['peripheralState'].fanSpeedRpm == 0 and self.sm['deviceState'].fanSpeedPercentDesired > 50: if (self.sm.frame - self.last_functional_fan_frame) * DT_CTRL > 5.0: self.events.add(EventName.fanMalfunction) @@ -279,8 +271,8 @@ def update_events(self, CS): self.events.add(EventName.preLaneChangeLeft) else: self.events.add(EventName.preLaneChangeRight) - elif self.sm['lateralPlan'].laneChangeState in [LaneChangeState.laneChangeStarting, - LaneChangeState.laneChangeFinishing]: + elif self.sm['lateralPlan'].laneChangeState in (LaneChangeState.laneChangeStarting, + LaneChangeState.laneChangeFinishing): self.events.add(EventName.laneChange) if not CS.canValid: @@ -289,9 +281,12 @@ def update_events(self, CS): for i, pandaState in enumerate(self.sm['pandaStates']): # All pandas must match the list of safetyConfigs, and if outside this list, must be silent or noOutput if i < len(self.CP.safetyConfigs): - safety_mismatch = pandaState.safetyModel != self.CP.safetyConfigs[i].safetyModel or pandaState.safetyParam != self.CP.safetyConfigs[i].safetyParam + safety_mismatch = pandaState.safetyModel != self.CP.safetyConfigs[i].safetyModel or \ + pandaState.safetyParam != self.CP.safetyConfigs[i].safetyParam or \ + pandaState.unsafeMode != self.CP.unsafeMode else: safety_mismatch = pandaState.safetyModel not in IGNORED_SAFETY_MODES + if safety_mismatch or self.mismatch_counter >= 200: self.events.add(EventName.controlsMismatch) @@ -308,7 +303,7 @@ def update_events(self, CS): if not self.logged_comm_issue: invalid = [s for s, valid in self.sm.valid.items() if not valid] not_alive = [s for s, alive in self.sm.alive.items() if not alive] - cloudlog.event("commIssue", invalid=invalid, not_alive=not_alive) + cloudlog.event("commIssue", invalid=invalid, not_alive=not_alive, can_error=self.can_rcv_error, error=True) self.logged_comm_issue = True else: self.logged_comm_issue = False @@ -340,25 +335,17 @@ def update_events(self, CS): self.events.add(EventName.fcw) if TICI: - logs = messaging.drain_sock(self.log_sock, wait_for_one=False) - messages = [] - for m in logs: + for m in messaging.drain_sock(self.log_sock, wait_for_one=False): try: - messages.append(m.androidLog.message) + msg = m.androidLog.message + if any(err in msg for err in ("ERROR_CRC", "ERROR_ECC", "ERROR_STREAM_UNDERFLOW", "APPLY FAILED")): + csid = msg.split("CSID:")[-1].split(" ")[0] + evt = CSID_MAP.get(csid, None) + if evt is not None: + self.events.add(evt) except UnicodeDecodeError: pass - for err in ["ERROR_CRC", "ERROR_ECC", "ERROR_STREAM_UNDERFLOW", "APPLY FAILED"]: - for m in messages: - if err not in m: - continue - - csid = m.split("CSID:")[-1].split(" ")[0] - evt = {"0": EventName.roadCameraError, "1": EventName.wideRoadCameraError, - "2": EventName.driverCameraError}.get(csid, None) - if evt is not None: - self.events.add(evt) - # TODO: fix simulator if not SIMULATION: if not NOSENSOR: @@ -446,12 +433,17 @@ def data_sample(self): self.sm.update(0) self.sm_smiskol.update(0) - # all_valid = CS.canValid and self.sm.all_alive_and_valid() - if not self.initialized: # and (all_valid or self.sm.frame * DT_CTRL > 3.5 or SIMULATION): - if not self.read_only: - self.CI.init(self.CP, self.can_sock, self.pm.sock['sendcan']) - self.initialized = True - Params().put_bool("ControlsReady", True) + if not self.initialized: + all_valid = CS.canValid and self.sm.all_alive_and_valid() + if all_valid or self.sm.frame * DT_CTRL > 3.5 or SIMULATION: + if not self.read_only: + self.CI.init(self.CP, self.can_sock, self.pm.sock['sendcan']) + self.initialized = True + + if REPLAY and self.sm['pandaStates'][0].controlsAllowed: + self.state = State.enabled + + Params().put_bool("ControlsReady", True) # Check for CAN timeout if not can_strs: @@ -468,7 +460,7 @@ def data_sample(self): self.mismatch_counter = 0 # All pandas not in silent mode must have controlsAllowed when openpilot is enabled - if any(not ps.controlsAllowed and self.enabled for ps in self.sm['pandaStates'] + if self.enabled and any(not ps.controlsAllowed for ps in self.sm['pandaStates'] if ps.safetyModel not in IGNORED_SAFETY_MODES): self.mismatch_counter += 1 @@ -518,7 +510,7 @@ def state_transition(self, CS): # no more soft disabling condition, so go back to ENABLED self.state = State.enabled - elif self.events.any(ET.SOFT_DISABLE) and self.soft_disable_timer > 0: + elif self.soft_disable_timer > 0: self.current_alert_types.append(ET.SOFT_DISABLE) elif self.soft_disable_timer <= 0: @@ -613,24 +605,14 @@ def state_control(self, CS): lac_log.output = steer lac_log.saturated = abs(steer) >= 0.9 - # Check for difference between desired angle and angle for angle based control - angle_control_saturated = self.CP.steerControlType == car.CarParams.SteerControlType.angle and \ - abs(actuators.steeringAngleDeg - CS.steeringAngleDeg) > STEER_ANGLE_SATURATION_THRESHOLD - - if angle_control_saturated and not CS.steeringPressed and self.active: - self.saturated_count += 1 - else: - self.saturated_count = 0 - # Send a "steering required alert" if saturation count has reached the limit - if (lac_log.saturated and not CS.steeringPressed) or \ - (self.saturated_count > STEER_ANGLE_SATURATION_TIMEOUT): - - if len(lat_plan.dPathPoints): + if lac_log.active and lac_log.saturated and not CS.steeringPressed: + dpath_points = lat_plan.dPathPoints + if len(dpath_points): # Check if we deviated from the path # TODO use desired vs actual curvature - left_deviation = actuators.steer > 0 and lat_plan.dPathPoints[0] < -0.20 - right_deviation = actuators.steer < 0 and lat_plan.dPathPoints[0] > 0.20 + left_deviation = actuators.steer > 0 and dpath_points[0] < -0.20 + right_deviation = actuators.steer < 0 and dpath_points[0] > 0.20 if left_deviation or right_deviation: self.events.add(EventName.steerSaturated) @@ -649,7 +631,7 @@ def state_control(self, CS): def update_button_timers(self, buttonEvents): # increment timer for buttons still pressed - for k in self.button_timers.keys(): + for k in self.button_timers: if self.button_timers[k] > 0: self.button_timers[k] += 1 @@ -706,10 +688,15 @@ def publish_logs(self, CS, start_time, actuators, lac_log): if hudControl.rightLaneDepart or hudControl.leftLaneDepart: self.events.add(EventName.ldw) - clear_event = ET.WARNING if ET.WARNING not in self.current_alert_types else None + clear_event_types = set() + if ET.WARNING not in self.current_alert_types: + clear_event_types.add(ET.WARNING) + if self.enabled: + clear_event_types.add(ET.NO_ENTRY) + alerts = self.events.create_alerts(self.current_alert_types, [self.CP, self.sm, self.is_metric, self.soft_disable_timer]) self.AM.add_many(self.sm.frame, alerts) - current_alert = self.AM.process_alerts(self.sm.frame, clear_event) + current_alert = self.AM.process_alerts(self.sm.frame, clear_event_types) if current_alert: hudControl.visualAlert = current_alert.visual_alert diff --git a/selfdrive/controls/lib/alertmanager.py b/selfdrive/controls/lib/alertmanager.py index 592047a5f0d2d9..70cc873ef3b6ea 100644 --- a/selfdrive/controls/lib/alertmanager.py +++ b/selfdrive/controls/lib/alertmanager.py @@ -42,12 +42,12 @@ def __init__(self): def add_many(self, frame: int, alerts: List[Alert]) -> None: for alert in alerts: - key = alert.alert_type - self.alerts[key].alert = alert - if not self.alerts[key].active(frame): - self.alerts[key].start_frame = frame - min_end_frame = self.alerts[key].start_frame + alert.duration - self.alerts[key].end_frame = max(frame + 1, min_end_frame) + entry = self.alerts[alert.alert_type] + entry.alert = alert + if not entry.active(frame): + entry.start_frame = frame + min_end_frame = entry.start_frame + alert.duration + entry.end_frame = max(frame + 1, min_end_frame) def SA_set_frame(self, frame): self.SA_frame = frame @@ -70,13 +70,13 @@ def SA_add(self, alert_name, extra_text_1='', extra_text_2=''): min_end_frame = self.alerts[alert.alert_type].start_frame + alert.duration self.alerts[alert.alert_type].end_frame = max(self.SA_frame + 1, min_end_frame) - def process_alerts(self, frame: int, clear_event_type=None) -> Optional[Alert]: + def process_alerts(self, frame: int, clear_event_types: set) -> Optional[Alert]: current_alert = AlertEntry() for v in self.alerts.values(): if not v.alert: continue - if clear_event_type and v.alert.event_type == clear_event_type: + if v.alert.event_type in clear_event_types: v.end_frame = -1 # sort by priority first and then by start_frame diff --git a/selfdrive/controls/lib/alerts_offroad.json b/selfdrive/controls/lib/alerts_offroad.json index 4affe0cfe30af5..aa54f582f68ee7 100644 --- a/selfdrive/controls/lib/alerts_offroad.json +++ b/selfdrive/controls/lib/alerts_offroad.json @@ -38,7 +38,7 @@ "severity": 1 }, "Offroad_StorageMissing": { - "text": "Storage drive not mounted.", + "text": "NVMe drive not mounted.", "severity": 1 }, "Offroad_CarUnrecognized": { diff --git a/selfdrive/controls/lib/desire_helper.py b/selfdrive/controls/lib/desire_helper.py new file mode 100644 index 00000000000000..a389d49642aee2 --- /dev/null +++ b/selfdrive/controls/lib/desire_helper.py @@ -0,0 +1,117 @@ +from cereal import log +from common.realtime import DT_MDL +from selfdrive.config import Conversions as CV +from common.op_params import opParams + +LaneChangeState = log.LateralPlan.LaneChangeState +LaneChangeDirection = log.LateralPlan.LaneChangeDirection + +LANE_CHANGE_SPEED_MIN = 30 * CV.MPH_TO_MS +LANE_CHANGE_TIME_MAX = 10. + +DESIRES = { + LaneChangeDirection.none: { + LaneChangeState.off: log.LateralPlan.Desire.none, + LaneChangeState.preLaneChange: log.LateralPlan.Desire.none, + LaneChangeState.laneChangeStarting: log.LateralPlan.Desire.none, + LaneChangeState.laneChangeFinishing: log.LateralPlan.Desire.none, + }, + LaneChangeDirection.left: { + LaneChangeState.off: log.LateralPlan.Desire.none, + LaneChangeState.preLaneChange: log.LateralPlan.Desire.none, + LaneChangeState.laneChangeStarting: log.LateralPlan.Desire.laneChangeLeft, + LaneChangeState.laneChangeFinishing: log.LateralPlan.Desire.laneChangeLeft, + }, + LaneChangeDirection.right: { + LaneChangeState.off: log.LateralPlan.Desire.none, + LaneChangeState.preLaneChange: log.LateralPlan.Desire.none, + LaneChangeState.laneChangeStarting: log.LateralPlan.Desire.laneChangeRight, + LaneChangeState.laneChangeFinishing: log.LateralPlan.Desire.laneChangeRight, + }, +} + + +class DesireHelper: + def __init__(self): + self.lane_change_state = LaneChangeState.off + self.lane_change_direction = LaneChangeDirection.none + self.lane_change_timer = 0.0 + self.lane_change_ll_prob = 1.0 + self.keep_pulse_timer = 0.0 + self.prev_one_blinker = False + self.desire = log.LateralPlan.Desire.none + self.op_params = opParams() + + def update(self, carstate, active, lane_change_prob): + v_ego = carstate.vEgo + one_blinker = carstate.leftBlinker != carstate.rightBlinker + below_lane_change_speed = v_ego < LANE_CHANGE_SPEED_MIN + + if not active or self.lane_change_timer > LANE_CHANGE_TIME_MAX: + self.lane_change_state = LaneChangeState.off + self.lane_change_direction = LaneChangeDirection.none + else: + # LaneChangeState.off + if self.lane_change_state == LaneChangeState.off and one_blinker and not self.prev_one_blinker and not below_lane_change_speed: + self.lane_change_state = LaneChangeState.preLaneChange + self.lane_change_ll_prob = 1.0 + + # LaneChangeState.preLaneChange + elif self.lane_change_state == LaneChangeState.preLaneChange: + # Set lane change direction + self.lane_change_direction = LaneChangeDirection.left if \ + carstate.leftBlinker else LaneChangeDirection.right + + torque_applied = carstate.steeringPressed and \ + ((carstate.steeringTorque > 0 and self.lane_change_direction == LaneChangeDirection.left) or + (carstate.steeringTorque < 0 and self.lane_change_direction == LaneChangeDirection.right)) + if v_ego >= self.op_params.get('alca_no_nudge_speed') * CV.MPH_TO_MS: + torque_applied = True + + blindspot_detected = ((carstate.leftBlindspot and self.lane_change_direction == LaneChangeDirection.left) or + (carstate.rightBlindspot and self.lane_change_direction == LaneChangeDirection.right)) + + if not one_blinker or below_lane_change_speed: + self.lane_change_state = LaneChangeState.off + elif torque_applied and not blindspot_detected: + self.lane_change_state = LaneChangeState.laneChangeStarting + + # LaneChangeState.laneChangeStarting + elif self.lane_change_state == LaneChangeState.laneChangeStarting: + # fade out over .5s + self.lane_change_ll_prob = max(self.lane_change_ll_prob - 2 * DT_MDL, 0.0) + + # 98% certainty + if lane_change_prob < 0.02 and self.lane_change_ll_prob < 0.01: + self.lane_change_state = LaneChangeState.laneChangeFinishing + + # LaneChangeState.laneChangeFinishing + elif self.lane_change_state == LaneChangeState.laneChangeFinishing: + # fade in laneline over 1s + self.lane_change_ll_prob = min(self.lane_change_ll_prob + DT_MDL, 1.0) + + if self.lane_change_ll_prob > 0.99: + self.lane_change_direction = LaneChangeDirection.none + if one_blinker: + self.lane_change_state = LaneChangeState.preLaneChange + else: + self.lane_change_state = LaneChangeState.off + + if self.lane_change_state in (LaneChangeState.off, LaneChangeState.preLaneChange): + self.lane_change_timer = 0.0 + else: + self.lane_change_timer += DT_MDL + + self.prev_one_blinker = one_blinker + + self.desire = DESIRES[self.lane_change_direction][self.lane_change_state] + + # Send keep pulse once per second during LaneChangeStart.preLaneChange + if self.lane_change_state in (LaneChangeState.off, LaneChangeState.laneChangeStarting): + self.keep_pulse_timer = 0.0 + elif self.lane_change_state == LaneChangeState.preLaneChange: + self.keep_pulse_timer += DT_MDL + if self.keep_pulse_timer > 1.0: + self.keep_pulse_timer = 0.0 + elif self.desire in (log.LateralPlan.Desire.keepLeft, log.LateralPlan.Desire.keepRight): + self.desire = log.LateralPlan.Desire.none diff --git a/selfdrive/controls/lib/events.py b/selfdrive/controls/lib/events.py index 48cdad524717c9..5f069c5f99fc00 100644 --- a/selfdrive/controls/lib/events.py +++ b/selfdrive/controls/lib/events.py @@ -63,10 +63,7 @@ def clear(self) -> None: self.events = self.static_events.copy() def any(self, event_type: str) -> bool: - for e in self.events: - if event_type in EVENTS.get(e, {}).keys(): - return True - return False + return any(event_type in EVENTS.get(e, {}) for e in self.events) def create_alerts(self, event_types: List[str], callback_args=None): if callback_args is None: @@ -96,7 +93,7 @@ def to_msg(self): for event_name in self.events: event = car.CarEvent.new_message() event.name = event_name - for event_type in EVENTS.get(event_name, {}).keys(): + for event_type in EVENTS.get(event_name, {}): setattr(event, event_type, True) ret.append(event) return ret @@ -239,7 +236,7 @@ def calibration_incomplete_alert(CP: car.CarParams, sm: messaging.SubMaster, met def no_gps_alert(CP: car.CarParams, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert: - gps_integrated = sm['peripheralState'].pandaType in [log.PandaState.PandaType.uno, log.PandaState.PandaType.dos] + gps_integrated = sm['peripheralState'].pandaType in (log.PandaState.PandaType.uno, log.PandaState.PandaType.dos) return Alert( "Poor GPS reception", "If sky is visible, contact support" if gps_integrated else "Check GPS antenna placement", @@ -267,8 +264,6 @@ def joystick_alert(CP: car.CarParams, sm: messaging.SubMaster, metric: bool, sof EventName.stockFcw: {}, - EventName.lkasDisabled: {}, - # ********** events only containing alerts displayed in all states ********** 'modelLongAlert': { @@ -379,14 +374,6 @@ def joystick_alert(CP: car.CarParams, sm: messaging.SubMaster, metric: bool, sof #ET.PERMANENT: ImmediateDisableAlert("openpilot failed to cancel cruise"), }, - # Some features or cars are marked as community features. If openpilot - # detects the use of a community feature it switches to dashcam mode - # until these features are allowed using a toggle in settings. - EventName.communityFeatureDisallowed: { - ET.PERMANENT: NormalPermanentAlert("openpilot Unavailable", - "Enable Community Features in Settings"), - }, - # openpilot doesn't recognize the car. This switches openpilot into a # read-only mode. This can be solved by adding your fingerprint. # See https://github.com/commaai/openpilot/wiki/Fingerprinting for more information @@ -894,4 +881,9 @@ def joystick_alert(CP: car.CarParams, sm: messaging.SubMaster, metric: bool, sof ET.NO_ENTRY: NoEntryAlert("Cruise Fault: Restart the Car"), }, + EventName.lkasDisabled: { + ET.PERMANENT: NormalPermanentAlert("LKAS Disabled: Enable LKAS to engage"), + ET.NO_ENTRY: NoEntryAlert("LKAS Disabled"), + }, + } diff --git a/selfdrive/controls/lib/lane_planner.py b/selfdrive/controls/lib/lane_planner.py index 26b3ec5949c269..eb1ee43b3f148d 100644 --- a/selfdrive/controls/lib/lane_planner.py +++ b/selfdrive/controls/lib/lane_planner.py @@ -10,12 +10,15 @@ TRAJECTORY_SIZE = 33 -# model path is 0.06 m left of car center -MODEL_PATH_OFFSET = 0.06 +# camera offset is meters from center car to camera +# model path is in the frame of EON's camera. TICI is 0.1 m away, +# however the average measured path difference is 0.04 m if EON: - CAMERA_OFFSET = 0.06 + CAMERA_OFFSET = -0.06 + PATH_OFFSET = 0.0 elif TICI: - CAMERA_OFFSET = -0.04 + CAMERA_OFFSET = 0.04 + PATH_OFFSET = 0.04 else: CAMERA_OFFSET = 0.0 @@ -43,20 +46,19 @@ def __init__(self, wide_camera=False): self.r_lane_change_prob = 0. self.camera_offset = -CAMERA_OFFSET if wide_camera else CAMERA_OFFSET - self.path_offset = self.camera_offset - MODEL_PATH_OFFSET + self.path_offset = 0 # self.camera_offset - MODEL_PATH_OFFSET def parse_model(self, md): lane_lines = md.laneLines if len(lane_lines) == 4 and len(lane_lines[0].t) == TRAJECTORY_SIZE: self.camera_offset = clip(self.op_params.get('camera_offset'), -0.5, 0.5) # update camera offset - self.path_offset = self.camera_offset - MODEL_PATH_OFFSET + # self.path_offset = self.camera_offset - MODEL_PATH_OFFSET self.ll_t = (np.array(lane_lines[1].t) + np.array(lane_lines[2].t))/2 # left and right ll x is the same self.ll_x = lane_lines[1].x - # only offset left and right lane lines; offsetting path does not make sense - self.lll_y = np.array(lane_lines[1].y) - self.camera_offset - self.rll_y = np.array(lane_lines[2].y) - self.camera_offset + self.lll_y = np.array(lane_lines[1].y) + self.camera_offset + self.rll_y = np.array(lane_lines[2].y) + self.camera_offset self.lll_prob = md.laneLineProbs[1] self.rll_prob = md.laneLineProbs[2] self.lll_std = md.laneLineStds[1] @@ -70,11 +72,11 @@ def parse_model(self, md): def get_d_path(self, v_ego, path_t, path_xyz): # Reduce reliance on lanelines that are too far apart or # will be in a few seconds - path_xyz[:, 1] -= self.path_offset + path_xyz[:, 1] += self.path_offset l_prob, r_prob = self.lll_prob, self.rll_prob width_pts = self.rll_y - self.lll_y prob_mods = [] - for t_check in [0.0, 1.5, 3.0]: + for t_check in (0.0, 1.5, 3.0): width_at_t = interp(t_check * (v_ego + 7), self.ll_x, width_pts) prob_mods.append(interp(width_at_t, [4.0, 5.0], [1.0, 0.0])) mod = min(prob_mods) diff --git a/selfdrive/controls/lib/latcontrol.py b/selfdrive/controls/lib/latcontrol.py new file mode 100644 index 00000000000000..eb16aca2e8c65f --- /dev/null +++ b/selfdrive/controls/lib/latcontrol.py @@ -0,0 +1,28 @@ +from abc import abstractmethod, ABC + +from common.realtime import DT_CTRL +from common.numpy_fast import clip + +MIN_STEER_SPEED = 0.3 + + +class LatControl(ABC): + def __init__(self, CP, CI): + self.sat_count_rate = 1.0 * DT_CTRL + self.sat_limit = CP.steerLimitTimer + self.sat_count = 0. + + @abstractmethod + def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate): + pass + + def reset(self): + self.sat_count = 0. + + def _check_saturation(self, saturated, CS): + if saturated and CS.vEgo > 10. and not CS.steeringRateLimited and not CS.steeringPressed: + self.sat_count += self.sat_count_rate + else: + self.sat_count -= self.sat_count_rate + self.sat_count = clip(self.sat_count, 0.0, self.sat_limit) + return self.sat_count > (self.sat_limit - 1e-3) diff --git a/selfdrive/controls/lib/latcontrol_angle.py b/selfdrive/controls/lib/latcontrol_angle.py index dc184cf58bdfd6..c9353563113a28 100644 --- a/selfdrive/controls/lib/latcontrol_angle.py +++ b/selfdrive/controls/lib/latcontrol_angle.py @@ -1,19 +1,16 @@ import math from cereal import log +from selfdrive.controls.lib.latcontrol import LatControl, MIN_STEER_SPEED +STEER_ANGLE_SATURATION_THRESHOLD = 2.5 # Degrees -class LatControlAngle(): - def __init__(self, CP): - pass - - def reset(self): - pass +class LatControlAngle(LatControl): def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate): angle_log = log.ControlsState.LateralAngleState.new_message() - if CS.vEgo < 0.3 or not active: + if CS.vEgo < MIN_STEER_SPEED or not active: angle_log.active = False angle_steers_des = float(CS.steeringAngleDeg) else: @@ -21,8 +18,8 @@ def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, angle_steers_des = math.degrees(VM.get_steer_from_curvature(-desired_curvature, CS.vEgo, params.roll)) angle_steers_des += params.angleOffsetDeg - angle_log.saturated = False + angle_control_saturated = abs(angle_steers_des - CS.steeringAngleDeg) > STEER_ANGLE_SATURATION_THRESHOLD + angle_log.saturated = self._check_saturation(angle_control_saturated, CS) angle_log.steeringAngleDeg = float(CS.steeringAngleDeg) angle_log.steeringAngleDesiredDeg = angle_steers_des - return 0, float(angle_steers_des), angle_log diff --git a/selfdrive/controls/lib/latcontrol_indi.py b/selfdrive/controls/lib/latcontrol_indi.py index 75b27ac8c12a4e..dc1b31bad9def9 100644 --- a/selfdrive/controls/lib/latcontrol_indi.py +++ b/selfdrive/controls/lib/latcontrol_indi.py @@ -5,13 +5,13 @@ from common.filter_simple import FirstOrderFilter from common.numpy_fast import clip, interp from common.realtime import DT_CTRL -from selfdrive.car import apply_toyota_steer_torque_limits -from selfdrive.car.toyota.values import CarControllerParams from selfdrive.controls.lib.drive_helpers import get_steer_max +from selfdrive.controls.lib.latcontrol import LatControl, MIN_STEER_SPEED -class LatControlINDI(): - def __init__(self, CP): +class LatControlINDI(LatControl): + def __init__(self, CP, CI): + super().__init__(CP, CI) self.angle_steers_des = 0. A = np.array([[1.0, DT_CTRL, 0.0], @@ -35,15 +35,11 @@ def __init__(self, CP): self.A_K = A - np.dot(K, C) self.x = np.array([[0.], [0.], [0.]]) - self.enforce_rate_limit = CP.carName == "toyota" - self._RC = (CP.lateralTuning.indi.timeConstantBP, CP.lateralTuning.indi.timeConstantV) self._G = (CP.lateralTuning.indi.actuatorEffectivenessBP, CP.lateralTuning.indi.actuatorEffectivenessV) self._outer_loop_gain = (CP.lateralTuning.indi.outerLoopGainBP, CP.lateralTuning.indi.outerLoopGainV) self._inner_loop_gain = (CP.lateralTuning.indi.innerLoopGainBP, CP.lateralTuning.indi.innerLoopGainV) - self.sat_count_rate = 1.0 * DT_CTRL - self.sat_limit = CP.steerLimitTimer self.steer_filter = FirstOrderFilter(0., self.RC, DT_CTRL) self.reset() @@ -65,24 +61,11 @@ def inner_loop_gain(self): return interp(self.speed, self._inner_loop_gain[0], self._inner_loop_gain[1]) def reset(self): + super().reset() self.steer_filter.x = 0. - self.output_steer = 0. - self.sat_count = 0. self.speed = 0. - def _check_saturation(self, control, check_saturation, limit): - saturated = abs(control) == limit - - if saturated and check_saturation: - self.sat_count += self.sat_count_rate - else: - self.sat_count -= self.sat_count_rate - - self.sat_count = clip(self.sat_count, 0.0, 1.0) - - return self.sat_count > self.sat_limit - - def update(self, active, CS, CP, VM, params, last_actuators, curvature, curvature_rate): + def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate): self.speed = CS.vEgo # Update Kalman filter y = np.array([[math.radians(CS.steeringAngleDeg)], [math.radians(CS.steeringRateDeg)]]) @@ -93,21 +76,21 @@ def update(self, active, CS, CP, VM, params, last_actuators, curvature, curvatur indi_log.steeringRateDeg = math.degrees(self.x[1]) indi_log.steeringAccelDeg = math.degrees(self.x[2]) - steers_des = VM.get_steer_from_curvature(-curvature, CS.vEgo, params.roll) + steers_des = VM.get_steer_from_curvature(-desired_curvature, CS.vEgo, params.roll) steers_des += math.radians(params.angleOffsetDeg) indi_log.steeringAngleDesiredDeg = math.degrees(steers_des) - rate_des = VM.get_steer_from_curvature(-curvature_rate, CS.vEgo, 0) + rate_des = VM.get_steer_from_curvature(-desired_curvature_rate, CS.vEgo, 0) indi_log.steeringRateDesiredDeg = math.degrees(rate_des) - if CS.vEgo < 0.3 or not active: + if CS.vEgo < MIN_STEER_SPEED or not active: indi_log.active = False - self.output_steer = 0.0 self.steer_filter.x = 0.0 + output_steer = 0 else: # Expected actuator value self.steer_filter.update_alpha(self.RC) - self.steer_filter.update(self.output_steer) + self.steer_filter.update(last_actuators.steer) # Compute acceleration error rate_sp = self.outer_loop_gain * (steers_des - self.x[0]) + rate_des @@ -119,21 +102,13 @@ def update(self, active, CS, CP, VM, params, last_actuators, curvature, curvatur delta_u = g_inv * accel_error # If steering pressed, only allow wind down - if CS.steeringPressed and (delta_u * self.output_steer > 0): + if CS.steeringPressed and (delta_u * last_actuators.steer > 0): delta_u = 0 - # Enforce rate limit - if self.enforce_rate_limit: - steer_max = float(CarControllerParams.STEER_MAX) - new_output_steer_cmd = steer_max * (self.steer_filter.x + delta_u) - prev_output_steer_cmd = steer_max * self.output_steer - new_output_steer_cmd = apply_toyota_steer_torque_limits(new_output_steer_cmd, prev_output_steer_cmd, prev_output_steer_cmd, CarControllerParams) - self.output_steer = new_output_steer_cmd / steer_max - else: - self.output_steer = self.steer_filter.x + delta_u + output_steer = self.steer_filter.x + delta_u steers_max = get_steer_max(CP, CS.vEgo) - self.output_steer = clip(self.output_steer, -steers_max, steers_max) + output_steer = clip(output_steer, -steers_max, steers_max) indi_log.active = True indi_log.rateSetPoint = float(rate_sp) @@ -141,9 +116,7 @@ def update(self, active, CS, CP, VM, params, last_actuators, curvature, curvatur indi_log.accelError = float(accel_error) indi_log.delayedOutput = float(self.steer_filter.x) indi_log.delta = float(delta_u) - indi_log.output = float(self.output_steer) - - check_saturation = (CS.vEgo > 10.) and not CS.steeringRateLimited and not CS.steeringPressed - indi_log.saturated = self._check_saturation(self.output_steer, check_saturation, steers_max) + indi_log.output = float(output_steer) + indi_log.saturated = self._check_saturation(steers_max - abs(output_steer) < 1e-3, CS) - return float(self.output_steer), float(steers_des), indi_log + return float(output_steer), float(steers_des), indi_log diff --git a/selfdrive/controls/lib/latcontrol_lqr.py b/selfdrive/controls/lib/latcontrol_lqr.py index 5777cde8e82f9b..5c273a45bedeb9 100644 --- a/selfdrive/controls/lib/latcontrol_lqr.py +++ b/selfdrive/controls/lib/latcontrol_lqr.py @@ -5,10 +5,12 @@ from common.realtime import DT_CTRL from cereal import log from selfdrive.controls.lib.drive_helpers import get_steer_max +from selfdrive.controls.lib.latcontrol import LatControl, MIN_STEER_SPEED -class LatControlLQR(): - def __init__(self, CP): +class LatControlLQR(LatControl): + def __init__(self, CP, CI): + super().__init__(CP, CI) self.scale = CP.lateralTuning.lqr.scale self.ki = CP.lateralTuning.lqr.ki @@ -23,26 +25,11 @@ def __init__(self, CP): self.i_unwind_rate = 0.3 * DT_CTRL self.i_rate = 1.0 * DT_CTRL - self.sat_count_rate = 1.0 * DT_CTRL - self.sat_limit = CP.steerLimitTimer - self.reset() def reset(self): + super().reset() self.i_lqr = 0.0 - self.sat_count = 0.0 - - def _check_saturation(self, control, check_saturation, limit): - saturated = abs(control) == limit - - if saturated and check_saturation: - self.sat_count += self.sat_count_rate - else: - self.sat_count -= self.sat_count_rate - - self.sat_count = clip(self.sat_count, 0.0, 1.0) - - return self.sat_count > self.sat_limit def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate): lqr_log = log.ControlsState.LateralLQRState.new_message() @@ -64,7 +51,7 @@ def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, e = steering_angle_no_offset - angle_steers_k self.x_hat = self.A.dot(self.x_hat) + self.B.dot(CS.steeringTorqueEps / torque_scale) + self.L.dot(e) - if CS.vEgo < 0.3 or not active: + if CS.vEgo < MIN_STEER_SPEED or not active: lqr_log.active = False lqr_output = 0. output_steer = 0. @@ -91,12 +78,9 @@ def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, output_steer = lqr_output + self.i_lqr output_steer = clip(output_steer, -steers_max, steers_max) - check_saturation = (CS.vEgo > 10) and not CS.steeringRateLimited and not CS.steeringPressed - saturated = self._check_saturation(output_steer, check_saturation, steers_max) - lqr_log.steeringAngleDeg = angle_steers_k lqr_log.i = self.i_lqr lqr_log.output = output_steer lqr_log.lqrOutput = lqr_output - lqr_log.saturated = saturated + lqr_log.saturated = self._check_saturation(steers_max - abs(output_steer) < 1e-3, CS) return output_steer, desired_angle, lqr_log diff --git a/selfdrive/controls/lib/latcontrol_model.py b/selfdrive/controls/lib/latcontrol_model.py index c31c76123f0b9d..c905410b787e10 100644 --- a/selfdrive/controls/lib/latcontrol_model.py +++ b/selfdrive/controls/lib/latcontrol_model.py @@ -8,7 +8,7 @@ class LatControlModel: - def __init__(self, CP): + def __init__(self, CP, CI): # Model generated using Konverter: https://github.com/sshane/Konverter model_weights_file = f'{BASEDIR}/models/steering/{CP.lateralTuning.model.name}_weights.npz' self.w, self.b = np.load(model_weights_file, allow_pickle=True)['wb'] diff --git a/selfdrive/controls/lib/latcontrol_pid.py b/selfdrive/controls/lib/latcontrol_pid.py index 7644c40c564c03..cd1b64763a95e1 100644 --- a/selfdrive/controls/lib/latcontrol_pid.py +++ b/selfdrive/controls/lib/latcontrol_pid.py @@ -2,20 +2,23 @@ from selfdrive.controls.lib.pid import PIDController from selfdrive.controls.lib.drive_helpers import get_steer_max +from selfdrive.controls.lib.latcontrol import LatControl, MIN_STEER_SPEED from cereal import log -class LatControlPID(): +class LatControlPID(LatControl): def __init__(self, CP, CI): + super().__init__(CP, CI) self.pid = PIDController((CP.lateralTuning.pid.kpBP, CP.lateralTuning.pid.kpV), - (CP.lateralTuning.pid.kiBP, CP.lateralTuning.pid.kiV), - (CP.lateralTuning.pid.kdBP, CP.lateralTuning.pid.kdV), - k_f=CP.lateralTuning.pid.kf, pos_limit=1.0, neg_limit=-1.0, - sat_limit=CP.steerLimitTimer, derivative_period=0.1) + (CP.lateralTuning.pid.kiBP, CP.lateralTuning.pid.kiV), + (CP.lateralTuning.pid.kdBP, CP.lateralTuning.pid.kdV), + k_f=CP.lateralTuning.pid.kf, pos_limit=1.0, neg_limit=-1.0, + derivative_period=0.1) self.new_kf_tuned = CP.lateralTuning.pid.newKfTuned self.get_steer_feedforward = CI.get_steer_feedforward_function() def reset(self): + super().reset() self.pid.reset() def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate): @@ -28,7 +31,7 @@ def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, pid_log.steeringAngleDesiredDeg = angle_steers_des pid_log.angleError = angle_steers_des - CS.steeringAngleDeg - if CS.vEgo < 0.3 or not active: + if CS.vEgo < MIN_STEER_SPEED or not active: output_steer = 0.0 pid_log.active = False self.pid.reset() @@ -47,14 +50,13 @@ def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, deadzone = 0.0 - check_saturation = (CS.vEgo > 10) and not CS.steeringRateLimited and not CS.steeringPressed - output_steer = self.pid.update(angle_steers_des, CS.steeringAngleDeg, check_saturation=check_saturation, override=CS.steeringPressed, + output_steer = self.pid.update(angle_steers_des, CS.steeringAngleDeg, override=CS.steeringPressed, feedforward=steer_feedforward, speed=CS.vEgo, deadzone=deadzone) pid_log.active = True pid_log.p = self.pid.p pid_log.i = self.pid.i pid_log.f = self.pid.f pid_log.output = output_steer - pid_log.saturated = bool(self.pid.saturated) + pid_log.saturated = self._check_saturation(steers_max - abs(output_steer) < 1e-3, CS) return output_steer, angle_steers_des, pid_log diff --git a/selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py b/selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py index 6162cf68a51ae3..eeda25627a0bef 100755 --- a/selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py +++ b/selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py @@ -3,6 +3,8 @@ import numpy as np from casadi import SX, vertcat, sin, cos + +from common.realtime import sec_since_boot from selfdrive.controls.lib.drive_helpers import LAT_MPC_N as N from selfdrive.controls.lib.drive_helpers import T_IDXS @@ -15,7 +17,8 @@ LAT_MPC_DIR = os.path.dirname(os.path.abspath(__file__)) EXPORT_DIR = os.path.join(LAT_MPC_DIR, "c_generated_code") JSON_FILE = "acados_ocp_lat.json" -X_DIM = 6 +X_DIM = 4 +P_DIM = 2 def gen_lat_model(): model = AcadosModel() @@ -26,9 +29,12 @@ def gen_lat_model(): y_ego = SX.sym('y_ego') psi_ego = SX.sym('psi_ego') curv_ego = SX.sym('curv_ego') + model.x = vertcat(x_ego, y_ego, psi_ego, curv_ego) + + # parameters v_ego = SX.sym('v_ego') rotation_radius = SX.sym('rotation_radius') - model.x = vertcat(x_ego, y_ego, psi_ego, curv_ego, v_ego, rotation_radius) + model.p = vertcat(v_ego, rotation_radius) # controls curv_rate = SX.sym('curv_rate') @@ -39,18 +45,14 @@ def gen_lat_model(): y_ego_dot = SX.sym('y_ego_dot') psi_ego_dot = SX.sym('psi_ego_dot') curv_ego_dot = SX.sym('curv_ego_dot') - v_ego_dot = SX.sym('v_ego_dot') - rotation_radius_dot = SX.sym('rotation_radius_dot') - model.xdot = vertcat(x_ego_dot, y_ego_dot, psi_ego_dot, curv_ego_dot, - v_ego_dot, rotation_radius_dot) + + model.xdot = vertcat(x_ego_dot, y_ego_dot, psi_ego_dot, curv_ego_dot) # dynamics model f_expl = vertcat(v_ego * cos(psi_ego) - rotation_radius * sin(psi_ego) * (v_ego * curv_ego), v_ego * sin(psi_ego) + rotation_radius * cos(psi_ego) * (v_ego * curv_ego), v_ego * curv_ego, - curv_rate, - 0.0, - 0.0) + curv_rate) model.f_impl_expr = model.xdot - f_expl model.f_expl_expr = f_expl return model @@ -77,8 +79,9 @@ def gen_lat_mpc_solver(): y_ego, psi_ego = ocp.model.x[1], ocp.model.x[2] curv_rate = ocp.model.u[0] - v_ego = ocp.model.x[4] + v_ego = ocp.model.p[0] + ocp.parameter_values = np.zeros((P_DIM, )) ocp.cost.yref = np.zeros((3, )) ocp.cost.yref_e = np.zeros((2, )) @@ -94,7 +97,7 @@ def gen_lat_mpc_solver(): ocp.constraints.idxbx = np.array([2,3]) ocp.constraints.ubx = np.array([np.radians(90), np.radians(50)]) ocp.constraints.lbx = np.array([-np.radians(90), -np.radians(50)]) - x0 = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0]) + x0 = np.zeros((X_DIM,)) ocp.constraints.x0 = x0 ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM' @@ -102,7 +105,7 @@ def gen_lat_mpc_solver(): ocp.solver_options.integrator_type = 'ERK' ocp.solver_options.nlp_solver_type = 'SQP_RTI' ocp.solver_options.qp_solver_iter_max = 1 - ocp.solver_options.qp_solver_cond_N = N//4 + ocp.solver_options.qp_solver_cond_N = 1 # set prediction horizon ocp.solver_options.tf = Tf @@ -128,10 +131,12 @@ def reset(self, x0=np.zeros(X_DIM)): # Somehow needed for stable init for i in range(N+1): self.solver.set(i, 'x', np.zeros(X_DIM)) + self.solver.set(i, 'p', np.zeros(P_DIM)) self.solver.constraints_set(0, "lbx", x0) self.solver.constraints_set(0, "ubx", x0) self.solver.solve() self.solution_status = 0 + self.solve_time = 0.0 self.cost = 0 def set_weights(self, path_weight, heading_weight, steer_rate_weight): @@ -141,17 +146,25 @@ def set_weights(self, path_weight, heading_weight, steer_rate_weight): #TODO hacky weights to keep behavior the same self.solver.cost_set(N, 'W', (3/20.)*W[:2,:2]) - def run(self, x0, v_ego, car_rotation_radius, y_pts, heading_pts): + def run(self, x0, p, y_pts, heading_pts): x0_cp = np.copy(x0) + p_cp = np.copy(p) self.solver.constraints_set(0, "lbx", x0_cp) self.solver.constraints_set(0, "ubx", x0_cp) self.yref[:,0] = y_pts + v_ego = p_cp[0] + # rotation_radius = p_cp[1] self.yref[:,1] = heading_pts*(v_ego+5.0) for i in range(N): self.solver.cost_set(i, "yref", self.yref[i]) + self.solver.set(i, "p", p_cp) + self.solver.set(N, "p", p_cp) self.solver.cost_set(N, "yref", self.yref[N][:2]) + t = sec_since_boot() self.solution_status = self.solver.solve() + self.solve_time = sec_since_boot() - t + for i in range(N+1): self.x_sol[i] = self.solver.get(i, 'x') for i in range(N): diff --git a/selfdrive/controls/lib/lateral_planner.py b/selfdrive/controls/lib/lateral_planner.py index 1af44b031903a5..bc4828c3780efb 100644 --- a/selfdrive/controls/lib/lateral_planner.py +++ b/selfdrive/controls/lib/lateral_planner.py @@ -1,4 +1,3 @@ -import math import numpy as np from common.realtime import sec_since_boot, DT_MDL from common.numpy_fast import interp @@ -6,56 +5,21 @@ from selfdrive.controls.lib.lateral_mpc_lib.lat_mpc import LateralMpc from selfdrive.controls.lib.drive_helpers import CONTROL_N, MPC_COST_LAT, LAT_MPC_N, CAR_ROTATION_RADIUS from selfdrive.controls.lib.lane_planner import LanePlanner, TRAJECTORY_SIZE -from selfdrive.config import Conversions as CV +from selfdrive.controls.lib.desire_helper import DesireHelper import cereal.messaging as messaging from cereal import log -from common.op_params import opParams - -LaneChangeState = log.LateralPlan.LaneChangeState -LaneChangeDirection = log.LateralPlan.LaneChangeDirection - -LANE_CHANGE_SPEED_MIN = 5 * CV.MPH_TO_MS -LANE_CHANGE_TIME_MAX = 10. - -DESIRES = { - LaneChangeDirection.none: { - LaneChangeState.off: log.LateralPlan.Desire.none, - LaneChangeState.preLaneChange: log.LateralPlan.Desire.none, - LaneChangeState.laneChangeStarting: log.LateralPlan.Desire.none, - LaneChangeState.laneChangeFinishing: log.LateralPlan.Desire.none, - }, - LaneChangeDirection.left: { - LaneChangeState.off: log.LateralPlan.Desire.none, - LaneChangeState.preLaneChange: log.LateralPlan.Desire.none, - LaneChangeState.laneChangeStarting: log.LateralPlan.Desire.laneChangeLeft, - LaneChangeState.laneChangeFinishing: log.LateralPlan.Desire.laneChangeLeft, - }, - LaneChangeDirection.right: { - LaneChangeState.off: log.LateralPlan.Desire.none, - LaneChangeState.preLaneChange: log.LateralPlan.Desire.none, - LaneChangeState.laneChangeStarting: log.LateralPlan.Desire.laneChangeRight, - LaneChangeState.laneChangeFinishing: log.LateralPlan.Desire.laneChangeRight, - }, -} class LateralPlanner: def __init__(self, CP, use_lanelines=True, wide_camera=False): self.use_lanelines = use_lanelines self.LP = LanePlanner(wide_camera) - self.op_params = opParams() + + self.DH = DesireHelper() self.last_cloudlog_t = 0 self.steer_rate_cost = CP.steerRateCost - self.solution_invalid_cnt = 0 - self.lane_change_state = LaneChangeState.off - self.lane_change_direction = LaneChangeDirection.none - self.lane_change_timer = 0.0 - self.lane_change_ll_prob = 1.0 - self.keep_pulse_timer = 0.0 - self.prev_one_blinker = False - self.desire = log.LateralPlan.Desire.none self.path_xyz = np.zeros((TRAJECTORY_SIZE, 3)) self.path_xyz_stds = np.ones((TRAJECTORY_SIZE, 3)) @@ -64,19 +28,19 @@ def __init__(self, CP, use_lanelines=True, wide_camera=False): self.y_pts = np.zeros(TRAJECTORY_SIZE) self.lat_mpc = LateralMpc() - self.reset_mpc(np.zeros(6)) + self.reset_mpc(np.zeros(4)) - def reset_mpc(self, x0=np.zeros(6)): + def reset_mpc(self, x0=np.zeros(4)): self.x0 = x0 self.lat_mpc.reset(x0=self.x0) def update(self, sm): v_ego = sm['carState'].vEgo - active = sm['controlsState'].active measured_curvature = sm['controlsState'].curvature + # Parse model predictions md = sm['modelV2'] - self.LP.parse_model(sm['modelV2']) + self.LP.parse_model(md) if len(md.position.x) == TRAJECTORY_SIZE and len(md.orientation.x) == TRAJECTORY_SIZE: self.path_xyz = np.column_stack([md.position.x, md.position.y, md.position.z]) self.t_idxs = np.array(md.position.t) @@ -85,86 +49,15 @@ def update(self, sm): self.path_xyz_stds = np.column_stack([md.position.xStd, md.position.yStd, md.position.zStd]) # Lane change logic - one_blinker = sm['carState'].leftBlinker != sm['carState'].rightBlinker - below_lane_change_speed = v_ego < LANE_CHANGE_SPEED_MIN - - if (not active) or (self.lane_change_timer > LANE_CHANGE_TIME_MAX): - self.lane_change_state = LaneChangeState.off - self.lane_change_direction = LaneChangeDirection.none - else: - # LaneChangeState.off - if self.lane_change_state == LaneChangeState.off and one_blinker and not self.prev_one_blinker and not below_lane_change_speed: - self.lane_change_state = LaneChangeState.preLaneChange - self.lane_change_ll_prob = 1.0 - - # LaneChangeState.preLaneChange - elif self.lane_change_state == LaneChangeState.preLaneChange: - # Set lane change direction - if sm['carState'].leftBlinker: - self.lane_change_direction = LaneChangeDirection.left - elif sm['carState'].rightBlinker: - self.lane_change_direction = LaneChangeDirection.right - else: # If there are no blinkers we will go back to LaneChangeState.off - self.lane_change_direction = LaneChangeDirection.none - - torque_applied = sm['carState'].steeringPressed and \ - ((sm['carState'].steeringTorque > 0 and self.lane_change_direction == LaneChangeDirection.left) or - (sm['carState'].steeringTorque < 0 and self.lane_change_direction == LaneChangeDirection.right)) - if v_ego >= self.op_params.get('alca_no_nudge_speed') * CV.MPH_TO_MS: - torque_applied = True - - blindspot_detected = ((sm['carState'].leftBlindspot and self.lane_change_direction == LaneChangeDirection.left) or - (sm['carState'].rightBlindspot and self.lane_change_direction == LaneChangeDirection.right)) - - if not one_blinker or below_lane_change_speed: - self.lane_change_state = LaneChangeState.off - elif torque_applied and not blindspot_detected: - self.lane_change_state = LaneChangeState.laneChangeStarting - - # LaneChangeState.laneChangeStarting - elif self.lane_change_state == LaneChangeState.laneChangeStarting: - # fade out over .5s - self.lane_change_ll_prob = max(self.lane_change_ll_prob - 2 * DT_MDL, 0.0) - - # 98% certainty - lane_change_prob = self.LP.l_lane_change_prob + self.LP.r_lane_change_prob - if lane_change_prob < 0.02 and self.lane_change_ll_prob < 0.01: - self.lane_change_state = LaneChangeState.laneChangeFinishing - - # LaneChangeState.laneChangeFinishing - elif self.lane_change_state == LaneChangeState.laneChangeFinishing: - # fade in laneline over 1s - self.lane_change_ll_prob = min(self.lane_change_ll_prob + DT_MDL, 1.0) - if self.lane_change_ll_prob > 0.99: - self.lane_change_direction = LaneChangeDirection.none - if one_blinker: - self.lane_change_state = LaneChangeState.preLaneChange - else: - self.lane_change_state = LaneChangeState.off - - if self.lane_change_state in [LaneChangeState.off, LaneChangeState.preLaneChange]: - self.lane_change_timer = 0.0 - else: - self.lane_change_timer += DT_MDL - - self.prev_one_blinker = one_blinker - - self.desire = DESIRES[self.lane_change_direction][self.lane_change_state] - - # Send keep pulse once per second during LaneChangeStart.preLaneChange - if self.lane_change_state in [LaneChangeState.off, LaneChangeState.laneChangeStarting]: - self.keep_pulse_timer = 0.0 - elif self.lane_change_state == LaneChangeState.preLaneChange: - self.keep_pulse_timer += DT_MDL - if self.keep_pulse_timer > 1.0: - self.keep_pulse_timer = 0.0 - elif self.desire in [log.LateralPlan.Desire.keepLeft, log.LateralPlan.Desire.keepRight]: - self.desire = log.LateralPlan.Desire.none + lane_change_prob = self.LP.l_lane_change_prob + self.LP.r_lane_change_prob + self.DH.update(sm['carState'], sm['controlsState'].active, lane_change_prob) # Turn off lanes during lane change - if self.desire == log.LateralPlan.Desire.laneChangeRight or self.desire == log.LateralPlan.Desire.laneChangeLeft: - self.LP.lll_prob *= self.lane_change_ll_prob - self.LP.rll_prob *= self.lane_change_ll_prob + if self.DH.desire == log.LateralPlan.Desire.laneChangeRight or self.DH.desire == log.LateralPlan.Desire.laneChangeLeft: + self.LP.lll_prob *= self.DH.lane_change_ll_prob + self.LP.rll_prob *= self.DH.lane_change_ll_prob + + # Calculate final driving path and set MPC costs if self.use_lanelines: d_path_xyz = self.LP.get_d_path(v_ego, self.t_idxs, self.path_xyz) self.lat_mpc.set_weights(MPC_COST_LAT.PATH, MPC_COST_LAT.HEADING, self.steer_rate_cost) @@ -174,23 +67,24 @@ def update(self, sm): # Heading cost is useful at low speed, otherwise end of plan can be off-heading heading_cost = interp(v_ego, [5.0, 10.0], [MPC_COST_LAT.HEADING, 0.0]) self.lat_mpc.set_weights(path_cost, heading_cost, self.steer_rate_cost) + y_pts = np.interp(v_ego * self.t_idxs[:LAT_MPC_N + 1], np.linalg.norm(d_path_xyz, axis=1), d_path_xyz[:, 1]) heading_pts = np.interp(v_ego * self.t_idxs[:LAT_MPC_N + 1], np.linalg.norm(self.path_xyz, axis=1), self.plan_yaw) self.y_pts = y_pts assert len(y_pts) == LAT_MPC_N + 1 assert len(heading_pts) == LAT_MPC_N + 1 - self.x0[4] = v_ego + # self.x0[4] = v_ego + p = np.array([v_ego, CAR_ROTATION_RADIUS]) self.lat_mpc.run(self.x0, - v_ego, - CAR_ROTATION_RADIUS, + p, y_pts, heading_pts) # init state for next self.x0[3] = interp(DT_MDL, self.t_idxs[:LAT_MPC_N + 1], self.lat_mpc.x_sol[:, 3]) # Check for infeasible MPC solution - mpc_nans = any(math.isnan(x) for x in self.lat_mpc.x_sol[:, 3]) + mpc_nans = np.isnan(self.lat_mpc.x_sol[:, 3]).any() t = sec_since_boot() if mpc_nans or self.lat_mpc.solution_status != 0: self.reset_mpc() @@ -211,9 +105,9 @@ def publish(self, sm, pm): lateralPlan = plan_send.lateralPlan lateralPlan.laneWidth = float(self.LP.lane_width) - lateralPlan.dPathPoints = [float(x) for x in self.y_pts] - lateralPlan.psis = [float(x) for x in self.lat_mpc.x_sol[0:CONTROL_N, 2]] - lateralPlan.curvatures = [float(x) for x in self.lat_mpc.x_sol[0:CONTROL_N, 3]] + lateralPlan.dPathPoints = self.y_pts.tolist() + lateralPlan.psis = self.lat_mpc.x_sol[0:CONTROL_N, 2].tolist() + lateralPlan.curvatures = self.lat_mpc.x_sol[0:CONTROL_N, 3].tolist() lateralPlan.curvatureRates = [float(x) for x in self.lat_mpc.u_sol[0:CONTROL_N - 1]] + [0.0] lateralPlan.lProb = float(self.LP.lll_prob) lateralPlan.rProb = float(self.LP.rll_prob) @@ -221,10 +115,11 @@ def publish(self, sm, pm): lateralPlan.cameraOffset = float(self.LP.camera_offset) lateralPlan.mpcSolutionValid = bool(plan_solution_valid) + lateralPlan.solverExecutionTime = self.lat_mpc.solve_time - lateralPlan.desire = self.desire + lateralPlan.desire = self.DH.desire lateralPlan.useLaneLines = self.use_lanelines - lateralPlan.laneChangeState = self.lane_change_state - lateralPlan.laneChangeDirection = self.lane_change_direction + lateralPlan.laneChangeState = self.DH.lane_change_state + lateralPlan.laneChangeDirection = self.DH.lane_change_direction pm.send('lateralPlan', plan_send) diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index e767c9b508f949..367bcb122a00c5 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -27,8 +27,7 @@ def long_control_state_trans(CP, active, long_control_state, v_ego, v_target_fut else: if long_control_state == LongCtrlState.off: - if active: - long_control_state = LongCtrlState.pid + long_control_state = LongCtrlState.pid elif long_control_state == LongCtrlState.pid: if stopping_condition: @@ -45,11 +44,10 @@ class LongControl(): def __init__(self, CP): self.long_control_state = LongCtrlState.off # initialized to off self.pid = PIDController((CP.longitudinalTuning.kpBP, CP.longitudinalTuning.kpV), - (CP.longitudinalTuning.kiBP, CP.longitudinalTuning.kiV), - (CP.longitudinalTuning.kdBP, CP.longitudinalTuning.kdV), - rate=1/DT_CTRL, - sat_limit=0.8, - derivative_period=0.5) + (CP.longitudinalTuning.kiBP, CP.longitudinalTuning.kiV), + (CP.longitudinalTuning.kdBP, CP.longitudinalTuning.kdV), + rate=1 / DT_CTRL, + derivative_period=0.5) self.v_pid = 0.0 self.last_output_accel = 0.0 diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 2c821ef33f337a..88efb6409bf2b0 100755 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -222,6 +222,7 @@ def reset(self): self.status = False self.crash_cnt = 0.0 self.solution_status = 0 + self.solve_time = 0.0 self.x0 = np.zeros(X_DIM) self.set_weights() @@ -387,7 +388,11 @@ def run(self): self.solver.set(i, 'p', self.params[i]) self.solver.constraints_set(0, "lbx", self.x0) self.solver.constraints_set(0, "ubx", self.x0) + + t = sec_since_boot() self.solution_status = self.solver.solve() + self.solve_time = sec_since_boot() - t + for i in range(N+1): self.x_sol[i] = self.solver.get(i, 'x') for i in range(N): @@ -399,7 +404,6 @@ def run(self): self.prev_a = np.interp(T_IDXS + 0.05, T_IDXS, self.a_solution) - t = sec_since_boot() if self.solution_status != 0: if t > self.last_cloudlog_t + 5.0: self.last_cloudlog_t = t diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 262777fee2afed..a91ddc0f12c652 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -112,12 +112,14 @@ def publish(self, sm, pm): longitudinalPlan.modelMonoTime = sm.logMonoTime['modelV2'] longitudinalPlan.processingDelay = (plan_send.logMonoTime / 1e9) - sm.logMonoTime['modelV2'] - longitudinalPlan.speeds = [float(x) for x in self.v_desired_trajectory] - longitudinalPlan.accels = [float(x) for x in self.a_desired_trajectory] - longitudinalPlan.jerks = [float(x) for x in self.j_desired_trajectory] + longitudinalPlan.speeds = self.v_desired_trajectory.tolist() + longitudinalPlan.accels = self.a_desired_trajectory.tolist() + longitudinalPlan.jerks = self.j_desired_trajectory.tolist() longitudinalPlan.hasLead = sm['radarState'].leadOne.status longitudinalPlan.longitudinalPlanSource = self.mpc.source longitudinalPlan.fcw = self.fcw + longitudinalPlan.solverExecutionTime = self.mpc.solve_time + pm.send('longitudinalPlan', plan_send) diff --git a/selfdrive/controls/lib/pid.py b/selfdrive/controls/lib/pid.py index 27650617a05467..9617248ef0326e 100644 --- a/selfdrive/controls/lib/pid.py +++ b/selfdrive/controls/lib/pid.py @@ -15,9 +15,8 @@ def apply_deadzone(error, deadzone): error = 0. return error - -class PIDController: - def __init__(self, k_p=0., k_i=0., k_d=0., k_f=1., pos_limit=None, neg_limit=None, rate=100, sat_limit=0.8, derivative_period=1.): +class PIDController(): + def __init__(self, k_p, k_i, k_d, k_f=1., pos_limit=None, neg_limit=None, rate=100, derivative_period=1.): self.op_params = opParams() self._k_p = k_p # proportional gain self._k_i = k_i # integral gain @@ -33,10 +32,8 @@ def __init__(self, k_p=0., k_i=0., k_d=0., k_f=1., pos_limit=None, neg_limit=Non self.pos_limit = pos_limit self.neg_limit = neg_limit - self.sat_count_rate = 1.0 / rate self.i_unwind_rate = 0.3 / rate self.i_rate = 1.0 / rate - self.sat_limit = sat_limit self._d_period = round(derivative_period * rate) # period of time for derivative calculation (seconds converted to frames) self.reset() @@ -53,28 +50,14 @@ def k_i(self): def k_d(self): return interp(self.speed, self._k_d[0], self._k_d[1]) - def _check_saturation(self, control, check_saturation, error): - saturated = (control < self.neg_limit) or (control > self.pos_limit) - - if saturated and check_saturation and abs(error) > 0.1: - self.sat_count += self.sat_count_rate - else: - self.sat_count -= self.sat_count_rate - - self.sat_count = clip(self.sat_count, 0.0, 1.0) - - return self.sat_count > self.sat_limit - def reset(self): self.p = 0.0 self.i = 0.0 self.f = 0.0 - self.sat_count = 0.0 - self.saturated = False self.control = 0 self.errors = [] - def update(self, setpoint, measurement, speed=0.0, check_saturation=True, override=False, feedforward=0., deadzone=0., freeze_integrator=False): + def update(self, setpoint, measurement, speed=0.0, override=False, feedforward=0., deadzone=0., freeze_integrator=False): self.speed = speed error = float(apply_deadzone(setpoint - measurement, deadzone)) @@ -100,7 +83,6 @@ def update(self, setpoint, measurement, speed=0.0, check_saturation=True, overri self.i = i control = self.p + self.f + self.i + d - self.saturated = self._check_saturation(control, check_saturation, error) self.errors.append(float(error)) while len(self.errors) > self._d_period: diff --git a/selfdrive/controls/lib/tests/test_alertmanager.py b/selfdrive/controls/lib/tests/test_alertmanager.py index 2b606390c78e3a..6c1b6fc4a2e3bc 100755 --- a/selfdrive/controls/lib/tests/test_alertmanager.py +++ b/selfdrive/controls/lib/tests/test_alertmanager.py @@ -34,7 +34,7 @@ def test_duration(self): for frame in range(duration+10): if frame < add_duration: AM.add_many(frame, [alert, ]) - current_alert = AM.process_alerts(frame) + current_alert = AM.process_alerts(frame, {}) shown = current_alert is not None should_show = frame <= show_duration diff --git a/selfdrive/controls/lib/tests/test_latcontrol.py b/selfdrive/controls/lib/tests/test_latcontrol.py new file mode 100755 index 00000000000000..8345840eca6063 --- /dev/null +++ b/selfdrive/controls/lib/tests/test_latcontrol.py @@ -0,0 +1,44 @@ +#!/usr/bin/env python3 +import unittest + +from parameterized import parameterized + +from cereal import car, log +from selfdrive.car.car_helpers import interfaces +from selfdrive.car.honda.values import CAR as HONDA +from selfdrive.car.toyota.values import CAR as TOYOTA +from selfdrive.car.nissan.values import CAR as NISSAN +from selfdrive.controls.lib.latcontrol_pid import LatControlPID +from selfdrive.controls.lib.latcontrol_lqr import LatControlLQR +from selfdrive.controls.lib.latcontrol_indi import LatControlINDI +from selfdrive.controls.lib.latcontrol_angle import LatControlAngle +from selfdrive.controls.lib.vehicle_model import VehicleModel + + +class TestLatControl(unittest.TestCase): + + @parameterized.expand([(HONDA.CIVIC, LatControlPID), (TOYOTA.RAV4, LatControlLQR), (TOYOTA.PRIUS, LatControlINDI), (NISSAN.LEAF, LatControlAngle)]) + def test_saturation(self, car_name, controller): + CarInterface, CarController, CarState = interfaces[car_name] + CP = CarInterface.get_params(car_name) + CI = CarInterface(CP, CarController, CarState) + VM = VehicleModel(CP) + + controller = controller(CP, CI) + + + CS = car.CarState.new_message() + CS.vEgo = 30 + + last_actuators = car.CarControl.Actuators.new_message() + + params = log.LiveParametersData.new_message() + + for _ in range(1000): + _, _, lac_log = controller.update(True, CS, CP, VM, params, last_actuators, 1, 0) + + self.assertTrue(lac_log.saturated) + + +if __name__ == "__main__": + unittest.main() diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index d4b0733692a360..65f8480c7c50e5 100755 --- a/selfdrive/controls/radard.py +++ b/selfdrive/controls/radard.py @@ -172,7 +172,7 @@ def update(self, sm, rr, enable_lead): radarState.carStateMonoTime = sm.logMonoTime['carState'] if enable_lead: - leads_v3 = sm['modelV2'].leadsV3 + leads_v3 = sm['modelV2'].leadsV3 if len(leads_v3) > 1: radarState.leadOne = get_lead(self.v_ego, self.ready, clusters, leads_v3[0], low_speed_override=True) radarState.leadTwo = get_lead(self.v_ego, self.ready, clusters, leads_v3[1], low_speed_override=False) diff --git a/selfdrive/controls/tests/test_alerts.py b/selfdrive/controls/tests/test_alerts.py index 46cbbab202d1bc..8de7a64cbcdf47 100755 --- a/selfdrive/controls/tests/test_alerts.py +++ b/selfdrive/controls/tests/test_alerts.py @@ -61,7 +61,7 @@ def test_alert_text_length(self): for alert in ALERTS: # for full size alerts, both text fields wrap the text, # so it's unlikely that they would go past the max width - if alert.alert_size in [AlertSize.none, AlertSize.full]: + if alert.alert_size in (AlertSize.none, AlertSize.full): continue for i, txt in enumerate([alert.alert_text_1, alert.alert_text_2]): diff --git a/selfdrive/controls/tests/test_lateral_mpc.py b/selfdrive/controls/tests/test_lateral_mpc.py index 7c4fe7b6ad18e8..4630e28a6154f5 100644 --- a/selfdrive/controls/tests/test_lateral_mpc.py +++ b/selfdrive/controls/tests/test_lateral_mpc.py @@ -14,12 +14,12 @@ def run_mpc(lat_mpc=None, v_ref=30., x_init=0., y_init=0., psi_init=0., curvatur y_pts = poly_shift * np.ones(LAT_MPC_N + 1) heading_pts = np.zeros(LAT_MPC_N + 1) - x0 = np.array([x_init, y_init, psi_init, curvature_init, v_ref, CAR_ROTATION_RADIUS]) + x0 = np.array([x_init, y_init, psi_init, curvature_init]) + p = np.array([v_ref, CAR_ROTATION_RADIUS]) # converge in no more than 10 iterations for _ in range(10): - lat_mpc.run(x0, v_ref, - CAR_ROTATION_RADIUS, + lat_mpc.run(x0, p, y_pts, heading_pts) return lat_mpc.x_sol diff --git a/selfdrive/controls/tests/test_startup.py b/selfdrive/controls/tests/test_startup.py index 658adf499eed5e..9d1345304503b2 100755 --- a/selfdrive/controls/tests/test_startup.py +++ b/selfdrive/controls/tests/test_startup.py @@ -42,31 +42,27 @@ class TestStartup(unittest.TestCase): # TODO: test EventName.startup for release branches # officially supported car - (EventName.startupMaster, TOYOTA.COROLLA, False, COROLLA_FW_VERSIONS), - (EventName.startupMaster, TOYOTA.COROLLA, True, COROLLA_FW_VERSIONS), - - # DSU unplugged - (EventName.startupMaster, TOYOTA.COROLLA, True, COROLLA_FW_VERSIONS_NO_DSU), - (EventName.communityFeatureDisallowed, TOYOTA.COROLLA, False, COROLLA_FW_VERSIONS_NO_DSU), + (EventName.startupMaster, TOYOTA.COROLLA, COROLLA_FW_VERSIONS), + (EventName.startupMaster, TOYOTA.COROLLA, COROLLA_FW_VERSIONS), # dashcamOnly car - (EventName.startupNoControl, MAZDA.CX5, True, CX5_FW_VERSIONS), - (EventName.startupNoControl, MAZDA.CX5, False, CX5_FW_VERSIONS), + (EventName.startupNoControl, MAZDA.CX5, CX5_FW_VERSIONS), + (EventName.startupNoControl, MAZDA.CX5, CX5_FW_VERSIONS), # unrecognized car with no fw - (EventName.startupNoFw, None, True, None), - (EventName.startupNoFw, None, False, None), + (EventName.startupNoFw, None, None), + (EventName.startupNoFw, None, None), # unrecognized car - (EventName.startupNoCar, None, True, COROLLA_FW_VERSIONS[:1]), - (EventName.startupNoCar, None, False, COROLLA_FW_VERSIONS[:1]), + (EventName.startupNoCar, None, COROLLA_FW_VERSIONS[:1]), + (EventName.startupNoCar, None, COROLLA_FW_VERSIONS[:1]), # fuzzy match - (EventName.startupMaster, TOYOTA.COROLLA, True, COROLLA_FW_VERSIONS_FUZZY), - (EventName.startupMaster, TOYOTA.COROLLA, False, COROLLA_FW_VERSIONS_FUZZY), + (EventName.startupMaster, TOYOTA.COROLLA, COROLLA_FW_VERSIONS_FUZZY), + (EventName.startupMaster, TOYOTA.COROLLA, COROLLA_FW_VERSIONS_FUZZY), ]) @with_processes(['controlsd']) - def test_startup_alert(self, expected_event, car_model, toggle_enabled, fw_versions): + def test_startup_alert(self, expected_event, car_model, fw_versions): # TODO: this should be done without any real sockets controls_sock = messaging.sub_sock("controlsState") @@ -76,7 +72,6 @@ def test_startup_alert(self, expected_event, car_model, toggle_enabled, fw_versi params.clear_all() params.put_bool("Passive", False) params.put_bool("OpenpilotEnabledToggle", True) - params.put_bool("CommunityFeaturesToggle", toggle_enabled) # Build capnn version of FW array if fw_versions is not None: diff --git a/selfdrive/crash.py b/selfdrive/crash.py deleted file mode 100644 index e49d843d94c190..00000000000000 --- a/selfdrive/crash.py +++ /dev/null @@ -1,45 +0,0 @@ -"""Install exception handler for process crash.""" -from selfdrive.swaglog import cloudlog -from selfdrive.version import get_version - -from selfdrive.version import get_origin, get_branch, is_dirty, get_commit -from common.op_params import opParams - -import sentry_sdk -from sentry_sdk import set_tag -from sentry_sdk.integrations.threading import ThreadingIntegration - -def capture_exception(*args, **kwargs) -> None: - cloudlog.error("crash", exc_info=kwargs.get('exc_info', 1)) - - try: - sentry_sdk.capture_exception(*args, **kwargs) - sentry_sdk.flush() # https://github.com/getsentry/sentry-python/issues/291 - except Exception: - cloudlog.exception("sentry exception") - -def capture_message(*args, **kwargs) -> None: - try: - sentry_sdk.capture_message(*args, **kwargs) - sentry_sdk.flush() # https://github.com/getsentry/sentry-python/issues/291 - except Exception: - cloudlog.exception("sentry exception") - -def bind_user(**kwargs) -> None: - sentry_sdk.set_user(kwargs) - -def bind_extra(**kwargs) -> None: - for k, v in kwargs.items(): - sentry_sdk.set_tag(k, v) - -def init() -> None: - sentry_uri = 'https://30d4f5e7d35c4a0d84455c03c0e80706@o237581.ingest.sentry.io/5844043' - - sentry_sdk.init(sentry_uri, default_integrations=False, - integrations=[ThreadingIntegration(propagate_hub=True)], release=get_version()) - - error_tags = {'dirty': is_dirty(fork=True), 'origin': get_origin(), 'branch': get_branch(), 'commit': get_commit(), 'username': opParams().get('username')} - if error_tags['username'] is None or not isinstance(error_tags['username'], str): - error_tags['username'] = 'undefined' - for tag in error_tags: - set_tag(tag, error_tags[tag]) diff --git a/selfdrive/debug/adb.sh b/selfdrive/debug/adb.sh index 82a3ddbb948ad5..8a04d4aefd583d 100755 --- a/selfdrive/debug/adb.sh +++ b/selfdrive/debug/adb.sh @@ -1,8 +1,16 @@ #!/usr/bin/bash +set -e -# then, connect to computer: -# adb connect 192.168.5.11:5555 +PORT=5555 -setprop service.adb.tcp.port 5555 -stop adbd -start adbd +setprop service.adb.tcp.port $PORT +if [ -f /EON ]; then + stop adbd + start adbd +else + sudo systemctl start adbd +fi + +IP=$(echo $SSH_CONNECTION | awk '{ print $3}') +echo "then, connect on your computer:" +echo "adb connect $IP:$PORT" diff --git a/selfdrive/debug/can_printer.py b/selfdrive/debug/can_printer.py index b4436bd2becdd8..0aaaf1350b0312 100755 --- a/selfdrive/debug/can_printer.py +++ b/selfdrive/debug/can_printer.py @@ -7,7 +7,7 @@ from common.realtime import sec_since_boot -def can_printer(bus, max_msg, addr): +def can_printer(bus, max_msg, addr, ascii_decode): logcan = messaging.sub_sock('can', addr=addr) start = sec_since_boot() @@ -24,10 +24,10 @@ def can_printer(bus, max_msg, addr): dd = chr(27) + "[2J" dd += f"{sec_since_boot() - start:5.2f}\n" for addr in sorted(msgs.keys()): - a = msgs[addr][-1].decode('ascii', 'backslashreplace') + a = f"\"{msgs[addr][-1].decode('ascii', 'backslashreplace')}\"" if ascii_decode else "" x = binascii.hexlify(msgs[addr][-1]).decode('ascii') if max_msg is None or addr < max_msg: - dd += "%04X(%4d)(%6d) %s \"%s\"\n" % (addr, addr, len(msgs[addr]), x.ljust(20), a) + dd += "%04X(%4d)(%6d) %s %s\n" % (addr, addr, len(msgs[addr]), x.ljust(20), a) print(dd) lp = sec_since_boot() @@ -36,8 +36,9 @@ def can_printer(bus, max_msg, addr): formatter_class=argparse.ArgumentDefaultsHelpFormatter) parser.add_argument("--bus", type=int, help="CAN bus to print out", default=0) - parser.add_argument("--max_msg", type=int, help="max addr ") + parser.add_argument("--max_msg", type=int, help="max addr") + parser.add_argument("--ascii", action='store_true', help="decode as ascii") parser.add_argument("--addr", default="127.0.0.1") args = parser.parse_args() - can_printer(args.bus, args.max_msg, args.addr) + can_printer(args.bus, args.max_msg, args.addr, args.ascii) diff --git a/selfdrive/debug/hyundai_enable_radar_points.py b/selfdrive/debug/hyundai_enable_radar_points.py index e566884a90df2e..8132a46552b996 100755 --- a/selfdrive/debug/hyundai_enable_radar_points.py +++ b/selfdrive/debug/hyundai_enable_radar_points.py @@ -13,47 +13,45 @@ import sys import argparse +from typing import NamedTuple from subprocess import check_output, CalledProcessError from panda.python import Panda from panda.python.uds import UdsClient, SESSION_TYPE, DATA_IDENTIFIER_TYPE +class ConfigValues(NamedTuple): + default_config: bytes + tracks_enabled: bytes + # If your radar supports changing data identifier 0x0142 as well make a PR to # this file to add your firmware version. Make sure to post a drive as proof! # NOTE: these firmware versions do not match what openpilot uses # because this script uses a different diagnostic session type SUPPORTED_FW_VERSIONS = { # 2020 SONATA - b"DN8_ SCC FHCUP 1.00 1.00 99110-L0000\x19\x08)\x15T ": { - "default_config": b"\x00\x00\x00\x01\x00\x00", - "tracks_enabled": b"\x00\x00\x00\x01\x00\x01", - }, + b"DN8_ SCC FHCUP 1.00 1.00 99110-L0000\x19\x08)\x15T ": ConfigValues( + default_config=b"\x00\x00\x00\x01\x00\x00", + tracks_enabled=b"\x00\x00\x00\x01\x00\x01"), # 2021 SONATA HYBRID - b"DNhe SCC FHCUP 1.00 1.02 99110-L5000 \x01#\x15# ": { - "default_config": b"\x00\x00\x00\x01\x00\x00", - "tracks_enabled": b"\x00\x00\x00\x01\x00\x01", - }, + b"DNhe SCC FHCUP 1.00 1.02 99110-L5000 \x01#\x15# ": ConfigValues( + default_config=b"\x00\x00\x00\x01\x00\x00", + tracks_enabled=b"\x00\x00\x00\x01\x00\x01"), # 2020 PALISADE - b"LX2_ SCC FHCUP 1.00 1.04 99110-S8100\x19\x05\x02\x16V ": { - "default_config": b"\x00\x00\x00\x01\x00\x00", - "tracks_enabled": b"\x00\x00\x00\x01\x00\x01", - }, + b"LX2_ SCC FHCUP 1.00 1.04 99110-S8100\x19\x05\x02\x16V ": ConfigValues( + default_config=b"\x00\x00\x00\x01\x00\x00", + tracks_enabled=b"\x00\x00\x00\x01\x00\x01"), # 2022 PALISADE - b"LX2_ SCC FHCUP 1.00 1.00 99110-S8110!\x04\x05\x17\x01 ": { - "default_config": b"\x00\x00\x00\x01\x00\x00", - "tracks_enabled": b"\x00\x00\x00\x01\x00\x01", - }, + b"LX2_ SCC FHCUP 1.00 1.00 99110-S8110!\x04\x05\x17\x01 ": ConfigValues( + default_config=b"\x00\x00\x00\x01\x00\x00", + tracks_enabled=b"\x00\x00\x00\x01\x00\x01"), # 2020 SANTA FE - b"TM__ SCC F-CUP 1.00 1.03 99110-S2000\x19\x050\x13' ": { - "default_config": b"\x00\x00\x00\x01\x00\x00", - "tracks_enabled": b"\x00\x00\x00\x01\x00\x01", - }, - + b"TM__ SCC F-CUP 1.00 1.03 99110-S2000\x19\x050\x13' ": ConfigValues( + default_config=b"\x00\x00\x00\x01\x00\x00", + tracks_enabled=b"\x00\x00\x00\x01\x00\x01"), # 2020 GENESIS G70 - b'IK__ SCC F-CUP 1.00 1.02 96400-G9100\x18\x07\x06\x17\x12 ': { - "default config": b"\x00\x00\x00\x01\x00\x00", - "tracks_enabled": b"\x00\x00\x00\x01\x00\x01", - }, + b'IK__ SCC F-CUP 1.00 1.02 96400-G9100\x18\x07\x06\x17\x12 ': ConfigValues( + default_config=b"\x00\x00\x00\x01\x00\x00", + tracks_enabled=b"\x00\x00\x00\x01\x00\x01"), } if __name__ == "__main__": @@ -95,13 +93,14 @@ print("[GET CONFIGURATION]") config_data_id : DATA_IDENTIFIER_TYPE = 0x0142 # type: ignore current_config = uds_client.read_data_by_identifier(config_data_id) - new_config = SUPPORTED_FW_VERSIONS[fw_version]["default_config" if args.default else "tracks_enabled"] + config_values = SUPPORTED_FW_VERSIONS[fw_version] + new_config = config_values.default_config if args.default else config_values.tracks_enabled print(f"current config: 0x{current_config.hex()}") if current_config != new_config: print("[CHANGE CONFIGURATION]") print(f"new config: 0x{new_config.hex()}") uds_client.write_data_by_identifier(config_data_id, new_config) - if not args.default and current_config != SUPPORTED_FW_VERSIONS[fw_version]["default_config"]: + if not args.default and current_config != SUPPORTED_FW_VERSIONS[fw_version].default_config: print("\ncurrent config does not match expected default! (aborted)") sys.exit(1) diff --git a/selfdrive/debug/profiling/snapdragon/.gitignore b/selfdrive/debug/profiling/snapdragon/.gitignore new file mode 100644 index 00000000000000..5d3033efb3db39 --- /dev/null +++ b/selfdrive/debug/profiling/snapdragon/.gitignore @@ -0,0 +1 @@ +SnapdragonProfiler/ diff --git a/selfdrive/debug/profiling/snapdragon/README b/selfdrive/debug/profiling/snapdragon/README new file mode 100644 index 00000000000000..ee826b413a635d --- /dev/null +++ b/selfdrive/debug/profiling/snapdragon/README @@ -0,0 +1,7 @@ +snapdragon profiler +-------- + +* download from https://developer.qualcomm.com/software/snapdragon-profiler +* unzip to selfdrive/debug/profiling/snapdragon/SnapdragonProfiler +* run ./setup-agnos.sh +* run selfdrive/debug/adb.sh on device diff --git a/selfdrive/debug/profiling/snapdragon/setup-agnos.sh b/selfdrive/debug/profiling/snapdragon/setup-agnos.sh new file mode 100755 index 00000000000000..5099fb09cbcd30 --- /dev/null +++ b/selfdrive/debug/profiling/snapdragon/setup-agnos.sh @@ -0,0 +1,7 @@ +#!/bin/bash + +# TODO: there's probably a better way to do this + +cd SnapdragonProfiler/service +mv android real_android +ln -s iot_rb5_lu/ android diff --git a/selfdrive/hardware/eon/androidd.py b/selfdrive/hardware/eon/androidd.py index b836eb01294dc6..dd26f64dced844 100755 --- a/selfdrive/hardware/eon/androidd.py +++ b/selfdrive/hardware/eon/androidd.py @@ -4,13 +4,14 @@ import psutil from typing import Optional +import cereal.messaging as messaging from common.realtime import set_core_affinity, set_realtime_priority from selfdrive.swaglog import cloudlog MAX_MODEM_CRASHES = 3 MODEM_PATH = "/sys/devices/soc/2080000.qcom,mss/subsys5" -WATCHED_PROCS = ["zygote", "zygote64", "/system/bin/servicemanager", "/system/bin/surfaceflinger"] +WATCHED_PROCS = ["zygote", "zygote64", "system_server", "/system/bin/servicemanager", "/system/bin/surfaceflinger"] def get_modem_crash_count() -> Optional[int]: @@ -37,6 +38,8 @@ def main(): crash_count = 0 modem_killed = False modem_state = "ONLINE" + androidLog = messaging.sub_sock('androidLog') + while True: # check critical android services if any(p is None or not p.is_running() for p in procs.values()) or not len(procs): @@ -49,9 +52,19 @@ def main(): if len(procs): for p in WATCHED_PROCS: if cur[p] != procs[p]: - cloudlog.event("android service pid changed", proc=p, cur=cur[p], prev=procs[p]) + cloudlog.event("android service pid changed", proc=p, cur=cur[p], prev=procs[p], error=True) procs.update(cur) + # log caught NetworkPolicy exceptions + msgs = messaging.drain_sock(androidLog) + for m in msgs: + try: + if m.androidLog.tag == "NetworkPolicy" and m.androidLog.message.startswith("problem with advise persist threshold"): + print(m) + cloudlog.event("network policy exception caught", androidLog=m.androidLog, error=True) + except UnicodeDecodeError: + pass + if os.path.exists(MODEM_PATH): # check modem state state = get_modem_state() @@ -68,7 +81,7 @@ def main(): # handle excessive modem crashes if crash_count > MAX_MODEM_CRASHES and not modem_killed: - cloudlog.event("killing modem") + cloudlog.event("killing modem", error=True) with open("/sys/kernel/debug/msm_subsys/modem", "w") as f: f.write("put") modem_killed = True diff --git a/selfdrive/hardware/eon/hardware.py b/selfdrive/hardware/eon/hardware.py index fa275c5a9c7ef8..4ab9f81fcf26cd 100644 --- a/selfdrive/hardware/eon/hardware.py +++ b/selfdrive/hardware/eon/hardware.py @@ -10,6 +10,12 @@ from cereal import log from selfdrive.hardware.base import HardwareBase, ThermalConfig +try: + from common.params import Params +except Exception: + # openpilot is not built yet + Params = None + NetworkType = log.DeviceState.NetworkType NetworkStrength = log.DeviceState.NetworkStrength @@ -70,6 +76,11 @@ def get_os_version(self): return f.read().strip() def get_device_type(self): + try: + if int(Params().get("LastPeripheralPandaType")) == log.PandaState.PandaType.uno: + return "two" + except Exception: + pass return "eon" def get_sound_card_online(self): diff --git a/selfdrive/hardware/eon/neos.json b/selfdrive/hardware/eon/neos.json index 228029e9fb7024..4010f7126a0892 100644 --- a/selfdrive/hardware/eon/neos.json +++ b/selfdrive/hardware/eon/neos.json @@ -1,7 +1,7 @@ { - "ota_url": "https://commadist.azureedge.net/neosupdate/ota-signed-5dc2575d713977666a8e14ae1b43a04d7f63123934c80fa10751d949a107653e.zip", - "ota_hash": "5dc2575d713977666a8e14ae1b43a04d7f63123934c80fa10751d949a107653e", - "recovery_url": "https://commadist.azureedge.net/neosupdate/recovery-f01a55c9ba52ca57668d1684c6bf4118efd31916b04f8c1fcd8495013d3677eb.img", + "ota_url": "https://commadist.azureedge.net/neosupdate/ota-signed-50da8800caa5cbc224acbaa21f3a83d21802a31d89cccfc62a898903a8eb19e7.zip", + "ota_hash": "50da8800caa5cbc224acbaa21f3a83d21802a31d89cccfc62a898903a8eb19e7", + "recovery_url": "https://commadist.azureedge.net/neosupdate/recovery-fe76739438d28ea6111853a737b616afdf340ba75c01c0ee99e6a28c19ecc29f.img", "recovery_len": 15222060, - "recovery_hash": "f01a55c9ba52ca57668d1684c6bf4118efd31916b04f8c1fcd8495013d3677eb" + "recovery_hash": "fe76739438d28ea6111853a737b616afdf340ba75c01c0ee99e6a28c19ecc29f" } diff --git a/selfdrive/hardware/eon/shutdownd.py b/selfdrive/hardware/eon/shutdownd.py new file mode 100755 index 00000000000000..e4ffa79ef31d3e --- /dev/null +++ b/selfdrive/hardware/eon/shutdownd.py @@ -0,0 +1,27 @@ +#!/usr/bin/env python3 +import os +import time +import datetime + +from common.params import Params +from selfdrive.hardware.eon.hardware import getprop +from selfdrive.swaglog import cloudlog + +def main(): + params = Params() + while True: + # 0 for shutdown, 1 for reboot + prop = getprop("sys.shutdown.requested") + if prop is not None and len(prop) > 0: + os.system("pkill -9 loggerd") + params.put("LastSystemShutdown", f"'{prop}' {datetime.datetime.now()}") + print("shutdown detected", repr(prop)) + + time.sleep(120) + cloudlog.error('shutdown false positive') + break + + time.sleep(0.1) + +if __name__ == "__main__": + main() diff --git a/selfdrive/hardware/hw.h b/selfdrive/hardware/hw.h index d7a6c8d1d5a60d..f18ede01ec25cc 100644 --- a/selfdrive/hardware/hw.h +++ b/selfdrive/hardware/hw.h @@ -20,17 +20,16 @@ class HardwarePC : public HardwareNone { #endif namespace Path { -inline static std::string HOME = util::getenv("HOME"); inline std::string log_root() { if (const char *env = getenv("LOG_ROOT")) { return env; } - return Hardware::PC() ? HOME + "/.comma/media/0/realdata" : "/data/media/0/realdata"; + return Hardware::PC() ? util::getenv("HOME") + "/.comma/media/0/realdata" : "/data/media/0/realdata"; } inline std::string params() { - return Hardware::PC() ? HOME + "/.comma/params" : "/data/params"; + return Hardware::PC() ? util::getenv("HOME") + "/.comma/params" : "/data/params"; } inline std::string rsa_file() { - return Hardware::PC() ? HOME + "/.comma/persist/comma/id_rsa" : "/persist/comma/id_rsa"; + return Hardware::PC() ? util::getenv("HOME") + "/.comma/persist/comma/id_rsa" : "/persist/comma/id_rsa"; } } // namespace Path diff --git a/selfdrive/hardware/tici/hardware.py b/selfdrive/hardware/tici/hardware.py index 855eee908efc62..31bfa736c031f7 100644 --- a/selfdrive/hardware/tici/hardware.py +++ b/selfdrive/hardware/tici/hardware.py @@ -320,6 +320,9 @@ def get_gpu_usage_percent(self): def initialize_hardware(self): self.amplifier.initialize_configuration() + # Allow thermald to write engagement status to kmsg + os.system("sudo chmod a+w /dev/kmsg") + def get_networks(self): r = {} diff --git a/selfdrive/locationd/calibrationd.py b/selfdrive/locationd/calibrationd.py index 61bc56b80e7698..ae191bbebbed3d 100755 --- a/selfdrive/locationd/calibrationd.py +++ b/selfdrive/locationd/calibrationd.py @@ -7,7 +7,6 @@ ''' import os -import copy from typing import NoReturn import numpy as np import cereal.messaging as messaging @@ -70,7 +69,7 @@ def __init__(self, param_put=False): if param_put and calibration_params: try: msg = log.Event.from_bytes(calibration_params) - rpy_init = list(msg.liveCalibration.rpyCalib) + rpy_init = np.array(msg.liveCalibration.rpyCalib) valid_blocks = msg.liveCalibration.validBlocks except Exception: cloudlog.exception("Error reading cached CalibrationParams") @@ -80,13 +79,15 @@ def __init__(self, param_put=False): def reset(self, rpy_init=RPY_INIT, valid_blocks=0, smooth_from=None): if not np.isfinite(rpy_init).all(): - self.rpy = copy.copy(RPY_INIT) + self.rpy = RPY_INIT.copy() else: - self.rpy = rpy_init + self.rpy = rpy_init.copy() + if not np.isfinite(valid_blocks) or valid_blocks < 0: - self.valid_blocks = 0 + self.valid_blocks = 0 else: self.valid_blocks = valid_blocks + self.rpys = np.tile(self.rpy, (INPUTS_WANTED, 1)) self.idx = 0 @@ -100,16 +101,19 @@ def reset(self, rpy_init=RPY_INIT, valid_blocks=0, smooth_from=None): self.old_rpy = smooth_from self.old_rpy_weight = 1.0 - def get_valid_idxs(self, ): + def get_valid_idxs(self): # exclude current block_idx from validity window before_current = list(range(self.block_idx)) after_current = list(range(min(self.valid_blocks, self.block_idx + 1), self.valid_blocks)) return before_current + after_current def update_status(self): - if len(self.get_valid_idxs()) > 0: - max_rpy_calib = np.array(np.max(self.rpys[self.get_valid_idxs()], axis=0)) - min_rpy_calib = np.array(np.min(self.rpys[self.get_valid_idxs()], axis=0)) + valid_idxs = self.get_valid_idxs() + if valid_idxs: + rpys = self.rpys[valid_idxs] + self.rpy = np.mean(rpys, axis=0) + max_rpy_calib = np.array(np.max(rpys, axis=0)) + min_rpy_calib = np.array(np.min(rpys, axis=0)) self.calib_spread = np.abs(max_rpy_calib - min_rpy_calib) else: self.calib_spread = np.zeros(3) @@ -164,8 +168,6 @@ def handle_cam_odom(self, trans, rot, trans_std, rot_std): self.block_idx += 1 self.valid_blocks = max(self.block_idx, self.valid_blocks) self.block_idx = self.block_idx % INPUTS_WANTED - if len(self.get_valid_idxs()) > 0: - self.rpy = np.mean(self.rpys[self.get_valid_idxs()], axis=0) self.update_status() @@ -179,9 +181,9 @@ def get_msg(self): msg.liveCalibration.validBlocks = self.valid_blocks msg.liveCalibration.calStatus = self.cal_status msg.liveCalibration.calPerc = min(100 * (self.valid_blocks * BLOCK_SIZE + self.idx) // (INPUTS_NEEDED * BLOCK_SIZE), 100) - msg.liveCalibration.extrinsicMatrix = [float(x) for x in extrinsic_matrix.flatten()] - msg.liveCalibration.rpyCalib = [float(x) for x in smooth_rpy] - msg.liveCalibration.rpyCalibSpread = [float(x) for x in self.calib_spread] + msg.liveCalibration.extrinsicMatrix = extrinsic_matrix.flatten().tolist() + msg.liveCalibration.rpyCalib = smooth_rpy.tolist() + msg.liveCalibration.rpyCalibSpread = self.calib_spread.tolist() return msg def send_data(self, pm) -> None: diff --git a/selfdrive/locationd/locationd.cc b/selfdrive/locationd/locationd.cc index 35fbd90e56860f..b723878e9d786c 100755 --- a/selfdrive/locationd/locationd.cc +++ b/selfdrive/locationd/locationd.cc @@ -80,11 +80,10 @@ void Localizer::build_live_location(cereal::LiveLocationKalman::Builder& fix) { VectorXd vel_ecef = predicted_state.segment(STATE_ECEF_VELOCITY_START); VectorXd vel_ecef_std = predicted_std.segment(STATE_ECEF_VELOCITY_ERR_START); VectorXd fix_pos_geo_vec = this->get_position_geodetic(); - //fix_pos_geo_std = np.abs(coord.ecef2geodetic(fix_ecef + fix_ecef_std) - fix_pos_geo) VectorXd orientation_ecef = quat2euler(vector2quat(predicted_state.segment(STATE_ECEF_ORIENTATION_START))); VectorXd orientation_ecef_std = predicted_std.segment(STATE_ECEF_ORIENTATION_ERR_START); + MatrixXdr orientation_ecef_cov = predicted_cov.block(STATE_ECEF_ORIENTATION_ERR_START, STATE_ECEF_ORIENTATION_ERR_START); MatrixXdr device_from_ecef = euler2rot(orientation_ecef).transpose(); - //VectorXd calibrated_orientation_ecef = rot2euler(device_from_ecef); VectorXd calibrated_orientation_ecef = rot2euler((this->calib_from_device * device_from_ecef).transpose()); VectorXd acc_calib = this->calib_from_device * predicted_state.segment(STATE_ACCELERATION_START); @@ -116,11 +115,10 @@ void Localizer::build_live_location(cereal::LiveLocationKalman::Builder& fix) { VectorXd vel_calib_std = rotate_cov(this->calib_from_device, vel_device_cov).diagonal().array().sqrt(); VectorXd orientation_ned = ned_euler_from_ecef(fix_ecef_ecef, orientation_ecef); - //orientation_ned_std = ned_euler_from_ecef(fix_ecef, orientation_ecef + orientation_ecef_std) - orientation_ned + VectorXd orientation_ned_std = rotate_cov(this->converter->ecef2ned_matrix, orientation_ecef_cov).diagonal().array().sqrt(); VectorXd calibrated_orientation_ned = ned_euler_from_ecef(fix_ecef_ecef, calibrated_orientation_ecef); VectorXd nextfix_ecef = fix_ecef + vel_ecef; VectorXd ned_vel = this->converter->ecef2ned((ECEF) { .x = nextfix_ecef(0), .y = nextfix_ecef(1), .z = nextfix_ecef(2) }).to_vector() - converter->ecef2ned(fix_ecef_ecef).to_vector(); - //ned_vel_std = self.converter->ecef2ned(fix_ecef + vel_ecef + vel_ecef_std) - self.converter->ecef2ned(fix_ecef + vel_ecef) VectorXd accDevice = predicted_state.segment(STATE_ACCELERATION_START); VectorXd accDeviceErr = predicted_std.segment(STATE_ACCELERATION_ERR_START); @@ -130,6 +128,7 @@ void Localizer::build_live_location(cereal::LiveLocationKalman::Builder& fix) { Vector3d nans = Vector3d(NAN, NAN, NAN); + // TODO fill in NED and Calibrated stds // write measurements to msg init_measurement(fix.initPositionGeodetic(), fix_pos_geo_vec, nans, this->gps_mode); init_measurement(fix.initPositionECEF(), fix_ecef, fix_ecef_std, this->gps_mode); @@ -139,7 +138,7 @@ void Localizer::build_live_location(cereal::LiveLocationKalman::Builder& fix) { init_measurement(fix.initAccelerationDevice(), accDevice, accDeviceErr, true); init_measurement(fix.initOrientationECEF(), orientation_ecef, orientation_ecef_std, this->gps_mode); init_measurement(fix.initCalibratedOrientationECEF(), calibrated_orientation_ecef, nans, this->calibrated && this->gps_mode); - init_measurement(fix.initOrientationNED(), orientation_ned, nans, this->gps_mode); + init_measurement(fix.initOrientationNED(), orientation_ned, orientation_ned_std, this->gps_mode); init_measurement(fix.initCalibratedOrientationNED(), calibrated_orientation_ned, nans, this->calibrated && this->gps_mode); init_measurement(fix.initAngularVelocityDevice(), angVelocityDevice, angVelocityDeviceErr, true); init_measurement(fix.initVelocityCalibrated(), vel_calib, vel_calib_std, this->calibrated); diff --git a/selfdrive/locationd/paramsd.py b/selfdrive/locationd/paramsd.py index 03de2d64622a9f..9557568ba28635 100755 --- a/selfdrive/locationd/paramsd.py +++ b/selfdrive/locationd/paramsd.py @@ -45,10 +45,12 @@ def handle_log(self, t, which, msg): yaw_rate_std = msg.angularVelocityCalibrated.std[2] localizer_roll = msg.orientationNED.value[0] + localizer_roll_std = msg.orientationNED.std[0] roll_valid = msg.orientationNED.valid and ROLL_MIN < localizer_roll < ROLL_MAX if roll_valid: roll = localizer_roll - roll_std = np.radians(1.0) + # Experimentally found multiplier of 2 to be best trade-off between stability and accuracy or similar? + roll_std = 2 * localizer_roll_std else: # This is done to bound the road roll estimate when localizer values are invalid roll = 0.0 diff --git a/selfdrive/locationd/test/test_calibrationd.py b/selfdrive/locationd/test/test_calibrationd.py index 030636dcd5bb2b..3612f48276a632 100755 --- a/selfdrive/locationd/test/test_calibrationd.py +++ b/selfdrive/locationd/test/test_calibrationd.py @@ -2,6 +2,8 @@ import random import unittest +import numpy as np + import cereal.messaging as messaging from common.params import Params from selfdrive.locationd.calibrationd import Calibrator @@ -16,7 +18,7 @@ def test_read_saved_params(self): Params().put("CalibrationParams", msg.to_bytes()) c = Calibrator(param_put=True) - self.assertEqual(list(msg.liveCalibration.rpyCalib), c.rpy) + np.testing.assert_allclose(msg.liveCalibration.rpyCalib, c.rpy) self.assertEqual(msg.liveCalibration.validBlocks, c.valid_blocks) diff --git a/selfdrive/locationd/ublox_msg.h b/selfdrive/locationd/ublox_msg.h index b8a48ad2da6636..da1c180c0b5264 100644 --- a/selfdrive/locationd/ublox_msg.h +++ b/selfdrive/locationd/ublox_msg.h @@ -23,11 +23,6 @@ namespace ublox { const int UBLOX_CHECKSUM_SIZE = 2; const int UBLOX_MAX_MSG_SIZE = 65536; - // Boardd still uses these: - const uint8_t CLASS_NAV = 0x01; - const uint8_t CLASS_RXM = 0x02; - const uint8_t CLASS_MON = 0x0A; - struct ubx_mga_ini_time_utc_t { uint8_t type; uint8_t version; diff --git a/selfdrive/logcatd/logcatd_android.cc b/selfdrive/logcatd/logcatd_android.cc index 4452e2f093ee28..1a982a0feb48d7 100644 --- a/selfdrive/logcatd/logcatd_android.cc +++ b/selfdrive/logcatd/logcatd_android.cc @@ -14,10 +14,8 @@ int main() { ExitHandler do_exit; PubMaster pm({"androidLog"}); - struct timespec cur_time; - clock_gettime(CLOCK_REALTIME, &cur_time); - log_time last_log_time(cur_time); - logger_list *logger_list = nullptr; + log_time last_log_time = {}; + logger_list *logger_list = android_logger_list_alloc(ANDROID_LOG_RDONLY | ANDROID_LOG_NONBLOCK, 0, 0); while (!do_exit) { // setup android logging diff --git a/selfdrive/logcatd/tests/test_logcatd_android.py b/selfdrive/logcatd/tests/test_logcatd_android.py index 5b908d3e875dfc..a98ae5834dbf85 100755 --- a/selfdrive/logcatd/tests/test_logcatd_android.py +++ b/selfdrive/logcatd/tests/test_logcatd_android.py @@ -34,7 +34,11 @@ def test_log(self): self.assertTrue(m.valid) self.assertLess(time.monotonic() - (m.logMonoTime / 1e9), 30) - recv_msg = m.androidLog.message.strip() + try: + recv_msg = m.androidLog.message.strip() + except UnicodeDecodeError: + continue + if recv_msg not in sent_msgs: continue diff --git a/selfdrive/loggerd/bootlog.cc b/selfdrive/loggerd/bootlog.cc index 478eb16852c4ef..c5897091dcaab5 100644 --- a/selfdrive/loggerd/bootlog.cc +++ b/selfdrive/loggerd/bootlog.cc @@ -23,20 +23,13 @@ static kj::Array build_boot_log() { std::string pstore = "/sys/fs/pstore"; std::map pstore_map = util::read_files_in_dir(pstore); - const std::vector log_keywords = {"Kernel panic"}; - auto lpstore = boot.initPstore().initEntries(pstore_map.size()); int i = 0; + auto lpstore = boot.initPstore().initEntries(pstore_map.size()); for (auto& kv : pstore_map) { auto lentry = lpstore[i]; lentry.setKey(kv.first); lentry.setValue(capnp::Data::Reader((const kj::byte*)kv.second.data(), kv.second.size())); i++; - - for (auto &k : log_keywords) { - if (kv.second.find(k) != std::string::npos) { - LOGE("%s: found '%s'", kv.first.c_str(), k.c_str()); - } - } } // Gather output of commands diff --git a/selfdrive/loggerd/config.py b/selfdrive/loggerd/config.py index b626073ce41b7d..6cd20a68ab40e7 100644 --- a/selfdrive/loggerd/config.py +++ b/selfdrive/loggerd/config.py @@ -13,6 +13,13 @@ CAMERA_FPS = 20 SEGMENT_LENGTH = 60 +STATS_DIR_FILE_LIMIT = 10000 +STATS_SOCKET = "ipc:///tmp/stats" +if PC: + STATS_DIR = os.path.join(str(Path.home()), ".comma", "stats") +else: + STATS_DIR = "/data/stats/" +STATS_FLUSH_TIME_S = 60 def get_available_percent(default=None): try: diff --git a/selfdrive/loggerd/loggerd.cc b/selfdrive/loggerd/loggerd.cc index 37f03ef4e51665..d402c5787f7523 100644 --- a/selfdrive/loggerd/loggerd.cc +++ b/selfdrive/loggerd/loggerd.cc @@ -11,14 +11,14 @@ bool sync_encoders(LoggerdState *s, CameraType cam_type, uint32_t frame_id) { update_max_atomic(s->start_frame_id, frame_id + 2); if (std::exchange(s->camera_ready[cam_type], true) == false) { ++s->encoders_ready; - LOGE("camera %d encoder ready", cam_type); + LOGD("camera %d encoder ready", cam_type); } return false; } else { if (s->max_waiting == 1) update_max_atomic(s->start_frame_id, frame_id); bool synced = frame_id >= s->start_frame_id; s->camera_synced[cam_type] = synced; - if (!synced) LOGE("camera %d waiting for frame %d, cur %d", cam_type, (int)s->start_frame_id, frame_id); + if (!synced) LOGD("camera %d waiting for frame %d, cur %d", cam_type, (int)s->start_frame_id, frame_id); return synced; } } @@ -244,7 +244,7 @@ void loggerd_thread() { count++; if (count >= 200) { - LOGE("large volume of '%s' messages", qs.name.c_str()); + LOGD("large volume of '%s' messages", qs.name.c_str()); break; } } diff --git a/selfdrive/loggerd/loggerd.h b/selfdrive/loggerd/loggerd.h index bdf5ef8f9d840f..caacbc591f90b6 100644 --- a/selfdrive/loggerd/loggerd.h +++ b/selfdrive/loggerd/loggerd.h @@ -80,7 +80,7 @@ const LogCameraInfo cameras_logged[] = { .downscale = false, .has_qcamera = false, .trigger_rotate = Hardware::TICI(), - .enable = !Hardware::PC(), + .enable = true, .record = Params().getBool("RecordFront"), }, { diff --git a/selfdrive/loggerd/raw_logger.cc b/selfdrive/loggerd/raw_logger.cc index b0f9f7a9c817e8..f103822a139d2b 100644 --- a/selfdrive/loggerd/raw_logger.cc +++ b/selfdrive/loggerd/raw_logger.cc @@ -11,6 +11,8 @@ #define __STDC_CONSTANT_MACROS +#include "libyuv.h" + extern "C" { #include #include @@ -55,6 +57,10 @@ RawLogger::RawLogger(const char* filename, int width, int height, int fps, frame->linesize[0] = width; frame->linesize[1] = width/2; frame->linesize[2] = width/2; + + if (downscale) { + downscale_buf.resize(width * height * 3 / 2); + } } RawLogger::~RawLogger() { @@ -64,7 +70,7 @@ RawLogger::~RawLogger() { } void RawLogger::encoder_open(const char* path) { - vid_path = util::string_format("%s/%s.mkv", path, filename); + vid_path = util::string_format("%s/%s", path, filename); // create camera lock file lock_path = util::string_format("%s/%s.lock", path, filename); @@ -76,7 +82,7 @@ void RawLogger::encoder_open(const char* path) { close(lock_fd); format_ctx = NULL; - avformat_alloc_output_context2(&format_ctx, NULL, NULL, vid_path.c_str()); + avformat_alloc_output_context2(&format_ctx, NULL, "matroska", vid_path.c_str()); assert(format_ctx); stream = avformat_new_stream(format_ctx, codec); @@ -124,9 +130,27 @@ int RawLogger::encode_frame(const uint8_t *y_ptr, const uint8_t *u_ptr, const ui pkt.data = NULL; pkt.size = 0; - frame->data[0] = (uint8_t*)y_ptr; - frame->data[1] = (uint8_t*)u_ptr; - frame->data[2] = (uint8_t*)v_ptr; + if (downscale_buf.size() > 0) { + uint8_t *out_y = downscale_buf.data(); + uint8_t *out_u = out_y + codec_ctx->width * codec_ctx->height; + uint8_t *out_v = out_u + (codec_ctx->width / 2) * (codec_ctx->height / 2); + libyuv::I420Scale(y_ptr, in_width, + u_ptr, in_width/2, + v_ptr, in_width/2, + in_width, in_height, + out_y, codec_ctx->width, + out_u, codec_ctx->width/2, + out_v, codec_ctx->width/2, + codec_ctx->width, codec_ctx->height, + libyuv::kFilterNone); + frame->data[0] = out_y; + frame->data[1] = out_u; + frame->data[2] = out_v; + } else { + frame->data[0] = (uint8_t*)y_ptr; + frame->data[1] = (uint8_t*)u_ptr; + frame->data[2] = (uint8_t*)v_ptr; + } frame->pts = counter; int ret = counter; diff --git a/selfdrive/loggerd/raw_logger.h b/selfdrive/loggerd/raw_logger.h index 9cef7ddcabda6e..75d906784d6877 100644 --- a/selfdrive/loggerd/raw_logger.h +++ b/selfdrive/loggerd/raw_logger.h @@ -39,4 +39,5 @@ class RawLogger : public VideoEncoder { AVFormatContext *format_ctx = NULL; AVFrame *frame = NULL; + std::vector downscale_buf; }; diff --git a/selfdrive/loggerd/tests/test_loggerd.py b/selfdrive/loggerd/tests/test_loggerd.py index 20cd61bad5d4f9..13e0b720d86bc9 100755 --- a/selfdrive/loggerd/tests/test_loggerd.py +++ b/selfdrive/loggerd/tests/test_loggerd.py @@ -1,4 +1,5 @@ #!/usr/bin/env python3 +import numpy as np import os import random import string @@ -14,12 +15,14 @@ from common.basedir import BASEDIR from common.params import Params from common.timeout import Timeout -from selfdrive.hardware import PC, TICI +from selfdrive.hardware import TICI from selfdrive.loggerd.config import ROOT from selfdrive.manager.process_config import managed_processes -from selfdrive.test.helpers import with_processes from selfdrive.version import get_version from tools.lib.logreader import LogReader +from cereal.visionipc.visionipc_pyx import VisionIpcServer, VisionStreamType # pylint: disable=no-name-in-module, import-error +from common.transformations.camera import eon_f_frame_size, tici_f_frame_size, \ + eon_d_frame_size, tici_d_frame_size, tici_e_frame_size SentinelType = log.Sentinel.SentinelType @@ -28,12 +31,6 @@ class TestLoggerd(unittest.TestCase): - # TODO: all tests should work on PC - @classmethod - def setUpClass(cls): - if PC: - raise unittest.SkipTest - def _get_latest_log_dir(self): log_dirs = sorted(Path(ROOT).iterdir(), key=lambda f: f.stat().st_mtime) return log_dirs[-1] @@ -107,25 +104,41 @@ def test_init_data_values(self): for _, k, v in fake_params: self.assertEqual(getattr(initData, k), v) - # TODO: this shouldn't need camerad - @with_processes(['camerad']) def test_rotation(self): os.environ["LOGGERD_TEST"] = "1" Params().put("RecordFront", "1") + expected_files = {"rlog.bz2", "qlog.bz2", "qcamera.ts", "fcamera.hevc", "dcamera.hevc"} + streams = [(VisionStreamType.VISION_STREAM_ROAD, tici_f_frame_size if TICI else eon_f_frame_size, "roadCameraState"), + (VisionStreamType.VISION_STREAM_DRIVER, tici_d_frame_size if TICI else eon_d_frame_size, "driverCameraState")] if TICI: expected_files.add("ecamera.hevc") + streams.append((VisionStreamType.VISION_STREAM_WIDE_ROAD, tici_e_frame_size, "wideRoadCameraState")) - # give camerad time to start - time.sleep(3) + pm = messaging.PubMaster(["roadCameraState", "driverCameraState", "wideRoadCameraState"]) + vipc_server = VisionIpcServer("camerad") + for stream_type, frame_size, _ in streams: + vipc_server.create_buffers(stream_type, 40, False, *(frame_size)) + vipc_server.start_listener() for _ in range(5): num_segs = random.randint(2, 5) length = random.randint(1, 3) os.environ["LOGGERD_SEGMENT_LENGTH"] = str(length) - managed_processes["loggerd"].start() - time.sleep(num_segs*length + 1) + + fps = 20.0 + for n in range(1, int(num_segs*length*fps)+1): + for stream_type, frame_size, state in streams: + dat = np.empty(int(frame_size[0]*frame_size[1]*3/2), dtype=np.uint8) + vipc_server.send(stream_type, dat[:].flatten().tobytes(), n, n/fps, n/fps) + + camera_state = messaging.new_message(state) + frame = getattr(camera_state, state) + frame.frameId = n + pm.send(state, camera_state) + time.sleep(1.0/fps) + managed_processes["loggerd"].stop() route_path = str(self._get_latest_log_dir()).rsplit("--", 1)[0] diff --git a/selfdrive/loggerd/xattr_cache.py b/selfdrive/loggerd/xattr_cache.py index aa97f0c777084c..e721692500536c 100644 --- a/selfdrive/loggerd/xattr_cache.py +++ b/selfdrive/loggerd/xattr_cache.py @@ -1,13 +1,15 @@ +from typing import Dict, Tuple + from common.xattr import getxattr as getattr1 from common.xattr import setxattr as setattr1 -cached_attributes = {} -def getxattr(path, attr_name): +cached_attributes: Dict[Tuple, bytes] = {} +def getxattr(path: str, attr_name: bytes) -> bytes: if (path, attr_name) not in cached_attributes: response = getattr1(path, attr_name) cached_attributes[(path, attr_name)] = response return cached_attributes[(path, attr_name)] -def setxattr(path, attr_name, attr_value): +def setxattr(path: str, attr_name: str, attr_value: bytes) -> None: cached_attributes.pop((path, attr_name), None) return setattr1(path, attr_name, attr_value) diff --git a/selfdrive/manager/manager.py b/selfdrive/manager/manager.py index 3c3d42df077bff..5e10c14cf845d2 100755 --- a/selfdrive/manager/manager.py +++ b/selfdrive/manager/manager.py @@ -8,7 +8,7 @@ from typing import List, Tuple, Union import cereal.messaging as messaging -import selfdrive.crash as crash +import selfdrive.sentry as sentry from common.basedir import BASEDIR from common.params import Params, ParamKeyType from common.text_window import TextWindow @@ -20,7 +20,7 @@ from selfdrive.athena.registration import register, UNREGISTERED_DONGLE_ID from selfdrive.swaglog import cloudlog, add_file_handler from selfdrive.version import is_dirty, get_commit, get_version, get_origin, get_short_branch, \ - terms_version, training_version, is_fork_remote + terms_version, training_version sys.path.append(os.path.join(BASEDIR, "pyextra")) @@ -90,16 +90,17 @@ def manager_init() -> None: if not is_dirty(): os.environ['CLEAN'] = '1' + # init logging + sentry.init(sentry.SentryProject.SELFDRIVE) cloudlog.bind_global(dongle_id=dongle_id, version=get_version(), dirty=is_dirty(), device=HARDWARE.get_device_type()) - # NOTE: All crash reports are sent to this fork maintainer's sentry, not comma - if is_fork_remote() and not (os.getenv("NOLOG") or os.getenv("NOCRASH") or PC): - crash.init() - crash.bind_user(id=dongle_id) - crash.bind_extra(dirty=is_dirty(fork=True), origin=get_origin(), branch=get_short_branch(), commit=get_commit(), - device=HARDWARE.get_device_type()) - + # # NOTE: All crash reports are sent to this fork maintainer's sentry, not comma + # if is_fork_remote() and not (os.getenv("NOLOG") or os.getenv("NOCRASH") or PC): + # crash.init() + # crash.bind_user(id=dongle_id) + # crash.bind_extra(dirty=is_dirty(fork=True), origin=get_origin(), branch=get_short_branch(), commit=get_commit(), + # device=HARDWARE.get_device_type()) def manager_prepare() -> None: for p in managed_processes.values(): @@ -125,8 +126,8 @@ def manager_thread() -> None: params = Params() - ignore = [] - if params.get("DongleId", encoding='utf8') == UNREGISTERED_DONGLE_ID: + ignore: List[str] = [] + if params.get("DongleId", encoding='utf8') in (None, UNREGISTERED_DONGLE_ID): ignore += ["manage_athenad", "uploader"] if os.getenv("NOBOARD") is not None: ignore.append("pandad") @@ -142,9 +143,6 @@ def manager_thread() -> None: sm.update() not_run = ignore[:] - if sm['deviceState'].freeSpacePercent < 5: - not_run.append("loggerd") - started = sm['deviceState'].started or sm['deviceState'].startedSentry driverview = params.get_bool("IsDriverViewEnabled") ensure_running(managed_processes.values(), started, driverview, sm['deviceState'].startedSentry, not_run) @@ -170,8 +168,9 @@ def manager_thread() -> None: shutdown = False for param in ("DoUninstall", "DoShutdown", "DoReboot"): if params.get_bool(param): - cloudlog.warning(f"Shutting down manager - {param} set") shutdown = True + params.put("LastManagerExitReason", param) + cloudlog.warning(f"Shutting down manager - {param} set") if shutdown: break @@ -198,7 +197,7 @@ def main() -> None: manager_thread() except Exception: traceback.print_exc() - crash.capture_exception() + sentry.capture_exception() finally: manager_cleanup() @@ -223,6 +222,11 @@ def main() -> None: add_file_handler(cloudlog) cloudlog.exception("Manager failed to start") + try: + managed_processes['ui'].stop() + except Exception: + pass + # Show last 3 lines of traceback error = traceback.format_exc(-3) error = "Manager failed to start\n\n" + error diff --git a/selfdrive/manager/process.py b/selfdrive/manager/process.py index 7785dda58f0d4b..cc3a454bebf98b 100644 --- a/selfdrive/manager/process.py +++ b/selfdrive/manager/process.py @@ -11,7 +11,7 @@ from setproctitle import setproctitle # pylint: disable=no-name-in-module import cereal.messaging as messaging -import selfdrive.crash as crash +import selfdrive.sentry as sentry from common.basedir import BASEDIR from common.params import Params from common.realtime import sec_since_boot @@ -34,8 +34,9 @@ def launcher(proc: str, name: str) -> None: # create new context since we forked messaging.context = messaging.Context() - # add daemon name to cloudlog ctx + # add daemon name tag to logs cloudlog.bind(daemon=name) + sentry.set_tag("daemon", name) # exec the process getattr(mod, 'main')() @@ -44,11 +45,13 @@ def launcher(proc: str, name: str) -> None: except Exception: # can't install the crash handler because sys.excepthook doesn't play nice # with threads, so catch it here. - crash.capture_exception() + sentry.capture_exception() raise -def nativelauncher(pargs: List[str], cwd: str) -> None: +def nativelauncher(pargs: List[str], cwd: str, name: str) -> None: + os.environ['MANAGER_DAEMON'] = name + # exec the process os.chdir(cwd) os.execvp(pargs[0], pargs) @@ -68,6 +71,7 @@ class ManagerProcess(ABC): sigkill = False persistent = False sentry = False + driverview = False proc: Optional[Process] = None enabled = True name = "" @@ -204,7 +208,7 @@ def start(self) -> None: cwd = os.path.join(BASEDIR, self.cwd) cloudlog.info(f"starting process {self.name}") - self.proc = Process(name=self.name, target=nativelauncher, args=(self.cmdline, cwd)) + self.proc = Process(name=self.name, target=nativelauncher, args=(self.cmdline, cwd, self.name)) self.proc.start() self.watchdog_seen = False self.shutting_down = False @@ -294,8 +298,7 @@ def ensure_running(procs: ValuesView[ManagerProcess], started: bool, driverview: p.stop(block=False) elif p.persistent: p.start() - elif getattr(p, 'driverview', False) and driverview: - # TODO: why is driverview an argument here? can this be done with the name? + elif p.driverview and driverview: p.start() # stop processes normally only starting when onroad from starting if not a sentry process elif not p.sentry and started_sentry: diff --git a/selfdrive/manager/process_config.py b/selfdrive/manager/process_config.py index 9963e57bb1d9f0..ae2d5b0c1c61ed 100644 --- a/selfdrive/manager/process_config.py +++ b/selfdrive/manager/process_config.py @@ -1,7 +1,7 @@ import os -from selfdrive.manager.process import PythonProcess, NativeProcess, DaemonProcess from selfdrive.hardware import EON, TICI, PC +from selfdrive.manager.process import PythonProcess, NativeProcess, DaemonProcess from common.op_params import opParams WEBCAM = os.getenv("USE_WEBCAM") is not None @@ -38,9 +38,11 @@ PythonProcess("timezoned", "selfdrive.timezoned", enabled=TICI, persistent=True), PythonProcess("tombstoned", "selfdrive.tombstoned", enabled=not PC, persistent=True), PythonProcess("uploader", "selfdrive.loggerd.uploader", persistent=True), + PythonProcess("statsd", "selfdrive.statsd", persistent=True), # EON only PythonProcess("rtshield", "selfdrive.rtshield", enabled=EON), + PythonProcess("shutdownd", "selfdrive.hardware.eon.shutdownd", enabled=EON), PythonProcess("androidd", "selfdrive.hardware.eon.androidd", enabled=EON, persistent=True), ] diff --git a/selfdrive/monitoring/dmonitoringd.py b/selfdrive/monitoring/dmonitoringd.py index 1728cc9e79fccb..f1b0c42bd98214 100755 --- a/selfdrive/monitoring/dmonitoringd.py +++ b/selfdrive/monitoring/dmonitoringd.py @@ -1,13 +1,19 @@ #!/usr/bin/env python3 +import gc + +import cereal.messaging as messaging from cereal import car from common.params import Params -import cereal.messaging as messaging +from common.realtime import set_realtime_priority from selfdrive.controls.lib.events import Events -from selfdrive.monitoring.driver_monitor import DriverStatus from selfdrive.locationd.calibrationd import Calibration +from selfdrive.monitoring.driver_monitor import DriverStatus def dmonitoringd_thread(sm=None, pm=None): + gc.disable() + set_realtime_priority(2) + if pm is None: pm = messaging.PubMaster(['driverMonitoringState']) @@ -38,8 +44,6 @@ def dmonitoringd_thread(sm=None, pm=None): v_cruise != v_cruise_last or \ sm['carState'].steeringPressed or \ sm['carState'].gasPressed - if driver_engaged: - driver_status.update(Events(), True, sm['controlsState'].enabled, sm['carState'].standstill) v_cruise_last = v_cruise if sm.updated['modelV2']: @@ -77,8 +81,10 @@ def dmonitoringd_thread(sm=None, pm=None): } pm.send('driverMonitoringState', dat) + def main(sm=None, pm=None): dmonitoringd_thread(sm, pm) + if __name__ == '__main__': main() diff --git a/selfdrive/monitoring/driver_monitor.py b/selfdrive/monitoring/driver_monitor.py index c470612dc0be8d..efb00ecb7723e5 100644 --- a/selfdrive/monitoring/driver_monitor.py +++ b/selfdrive/monitoring/driver_monitor.py @@ -205,8 +205,8 @@ def set_policy(self, model_data, car_speed): self.settings._POSE_YAW_THRESHOLD_STRICT]) / self.settings._POSE_YAW_THRESHOLD def get_pose(self, driver_state, cal_rpy, car_speed, op_engaged): - if not all(len(x) > 0 for x in [driver_state.faceOrientation, driver_state.facePosition, - driver_state.faceOrientationStd, driver_state.facePositionStd]): + if not all(len(x) > 0 for x in (driver_state.faceOrientation, driver_state.facePosition, + driver_state.faceOrientationStd, driver_state.facePositionStd)): return self.face_partial = driver_state.partialFace > self.settings._PARTIAL_FACE_THRESHOLD diff --git a/selfdrive/pandad.py b/selfdrive/pandad.py index e3ebdad6374710..1f9fddab7af87a 100755 --- a/selfdrive/pandad.py +++ b/selfdrive/pandad.py @@ -13,7 +13,7 @@ from selfdrive.swaglog import cloudlog -def get_expected_signature(panda : Panda) -> bytes: +def get_expected_signature(panda: Panda) -> bytes: fn = DEFAULT_H7_FW_FN if (panda.get_mcu_type() == MCU_TYPE_H7) else DEFAULT_FW_FN try: @@ -23,7 +23,7 @@ def get_expected_signature(panda : Panda) -> bytes: return b"" -def flash_panda(panda_serial : str) -> Panda: +def flash_panda(panda_serial: str) -> Panda: panda = Panda(panda_serial) fw_signature = get_expected_signature(panda) @@ -54,7 +54,8 @@ def flash_panda(panda_serial : str) -> Panda: return panda -def panda_sort_cmp(a : Panda, b : Panda): + +def panda_sort_cmp(a: Panda, b: Panda): a_type = a.get_type() b_type = b.get_type() @@ -71,9 +72,15 @@ def panda_sort_cmp(a : Panda, b : Panda): # last resort: sort by serial number return a.get_usb_serial() < b.get_usb_serial() + def main() -> NoReturn: + first_run = True + params = Params() + while True: try: + params.delete("PandaSignatures") + # Flash all Pandas in DFU mode for p in PandaDFU.list(): cloudlog.info(f"Panda in DFU mode found, flashing recovery {p}") @@ -95,16 +102,20 @@ def main() -> NoReturn: for panda in pandas: health = panda.health() if health["heartbeat_lost"]: - Params().put_bool("PandaHeartbeatLost", True) + params.put_bool("PandaHeartbeatLost", True) cloudlog.event("heartbeat lost", deviceState=health, serial=panda.get_usb_serial()) - cloudlog.info(f"Resetting panda {panda.get_usb_serial()}") - panda.reset() + if first_run: + cloudlog.info(f"Resetting panda {panda.get_usb_serial()}") + panda.reset() # sort pandas to have deterministic order pandas.sort(key=cmp_to_key(panda_sort_cmp)) panda_serials = list(map(lambda p: p.get_usb_serial(), pandas)) + # log panda fw versions + params.put("PandaSignatures", b','.join(p.get_signature() for p in pandas)) + # close all pandas for p in pandas: p.close() @@ -113,6 +124,8 @@ def main() -> NoReturn: cloudlog.exception("Panda USB exception while setting up") continue + first_run = False + # run boardd with all connected serials as arguments os.chdir(os.path.join(BASEDIR, "selfdrive/boardd")) subprocess.run(["./boardd", *panda_serials], check=True) diff --git a/selfdrive/sentry.py b/selfdrive/sentry.py new file mode 100644 index 00000000000000..5f22bf18e0f706 --- /dev/null +++ b/selfdrive/sentry.py @@ -0,0 +1,75 @@ +"""Install exception handler for process crash.""" +import sentry_sdk +from enum import Enum +from sentry_sdk.integrations.threading import ThreadingIntegration + +from common.params import Params +from selfdrive.athena.registration import is_registered_device +from selfdrive.hardware import HARDWARE, PC +from selfdrive.swaglog import cloudlog +from selfdrive.version import get_branch, get_commit, get_origin, get_version, \ + is_comma_remote, is_dirty, is_tested_branch + + +class SentryProject(Enum): + # python project + SELFDRIVE = "https://6f3c7076c1e14b2aa10f5dde6dda0cc4@o33823.ingest.sentry.io/77924" + # native project + SELFDRIVE_NATIVE = "https://3e4b586ed21a4479ad5d85083b639bc6@o33823.ingest.sentry.io/157615" + + +def report_tombstone(fn: str, message: str, contents: str) -> None: + cloudlog.error({'tombstone': message}) + + with sentry_sdk.configure_scope() as scope: + scope.set_extra("tombstone_fn", fn) + scope.set_extra("tombstone", contents) + sentry_sdk.capture_message(message=message) + sentry_sdk.flush() + + +def capture_exception(*args, **kwargs) -> None: + cloudlog.error("crash", exc_info=kwargs.get('exc_info', 1)) + + try: + sentry_sdk.capture_exception(*args, **kwargs) + sentry_sdk.flush() # https://github.com/getsentry/sentry-python/issues/291 + except Exception: + cloudlog.exception("sentry exception") + + +def set_tag(key: str, value: str) -> None: + sentry_sdk.set_tag(key, value) + + +def init(project: SentryProject) -> None: + # forks like to mess with this, so double check + comma_remote = is_comma_remote() and "commaai" in get_origin(default="") + if not comma_remote or not is_registered_device() or PC: + return + + env = "release" if is_tested_branch() else "master" + dongle_id = Params().get("DongleId", encoding='utf-8') + + integrations = [] + if project == SentryProject.SELFDRIVE: + integrations.append(ThreadingIntegration(propagate_hub=True)) + else: + sentry_sdk.utils.MAX_STRING_LENGTH = 8192 + + sentry_sdk.init(project.value, + default_integrations=False, + release=get_version(), + integrations=integrations, + traces_sample_rate=1.0, + environment=env) + + sentry_sdk.set_user({"id": dongle_id}) + sentry_sdk.set_tag("dirty", is_dirty()) + sentry_sdk.set_tag("origin", get_origin()) + sentry_sdk.set_tag("branch", get_branch()) + sentry_sdk.set_tag("commit", get_commit()) + sentry_sdk.set_tag("device", HARDWARE.get_device_type()) + + if project == SentryProject.SELFDRIVE: + sentry_sdk.Hub.current.start_session() diff --git a/selfdrive/statsd.py b/selfdrive/statsd.py new file mode 100755 index 00000000000000..2e62e32536d42b --- /dev/null +++ b/selfdrive/statsd.py @@ -0,0 +1,126 @@ +#!/usr/bin/env python3 +import os +import zmq +import time +from pathlib import Path +from datetime import datetime, timezone +from typing import NoReturn + +from common.params import Params +from cereal.messaging import SubMaster +from selfdrive.swaglog import cloudlog +from selfdrive.hardware import HARDWARE +from common.file_helpers import atomic_write_in_dir +from selfdrive.version import get_normalized_origin, get_short_branch, get_short_version, is_dirty +from selfdrive.loggerd.config import STATS_DIR, STATS_DIR_FILE_LIMIT, STATS_SOCKET, STATS_FLUSH_TIME_S + + +class METRIC_TYPE: + GAUGE = 'g' + +class StatLog: + def __init__(self): + self.pid = None + + def connect(self) -> None: + self.zctx = zmq.Context() + self.sock = self.zctx.socket(zmq.PUSH) + self.sock.setsockopt(zmq.LINGER, 10) + self.sock.connect(STATS_SOCKET) + self.pid = os.getpid() + + def _send(self, metric: str) -> None: + if os.getpid() != self.pid: + self.connect() + + try: + self.sock.send_string(metric, zmq.NOBLOCK) + except zmq.error.Again: + # drop :/ + pass + + def gauge(self, name: str, value: float) -> None: + self._send(f"{name}:{value}|{METRIC_TYPE.GAUGE}") + + +def main() -> NoReturn: + dongle_id = Params().get("DongleId", encoding='utf-8') + def get_influxdb_line(measurement: str, value: float, timestamp: datetime, tags: dict) -> str: + res = f"{measurement}" + for k, v in tags.items(): + res += f",{k}={str(v)}" + res += f" value={value},dongle_id=\"{dongle_id}\" {int(timestamp.timestamp() * 1e9)}\n" + return res + + # open statistics socket + ctx = zmq.Context().instance() + sock = ctx.socket(zmq.PULL) + sock.bind(STATS_SOCKET) + + # initialize stats directory + Path(STATS_DIR).mkdir(parents=True, exist_ok=True) + + # initialize tags + tags = { + 'started': False, + 'version': get_short_version(), + 'branch': get_short_branch(), + 'dirty': is_dirty(), + 'origin': get_normalized_origin(), + 'deviceType': HARDWARE.get_device_type(), + } + + # subscribe to deviceState for started state + sm = SubMaster(['deviceState']) + + last_flush_time = time.monotonic() + gauges = {} + while True: + started_prev = sm['deviceState'].started + sm.update() + + # Update metrics + while True: + try: + metric = sock.recv_string(zmq.NOBLOCK) + try: + metric_type = metric.split('|')[1] + metric_name = metric.split(':')[0] + metric_value = metric.split('|')[0].split(':')[1] + + if metric_type == METRIC_TYPE.GAUGE: + gauges[metric_name] = metric_value + else: + cloudlog.event("unknown metric type", metric_type=metric_type) + except Exception: + cloudlog.event("malformed metric", metric=metric) + except zmq.error.Again: + break + + # flush when started state changes or after FLUSH_TIME_S + if (time.monotonic() > last_flush_time + STATS_FLUSH_TIME_S) or (sm['deviceState'].started != started_prev): + result = "" + current_time = datetime.utcnow().replace(tzinfo=timezone.utc) + tags['started'] = sm['deviceState'].started + + for gauge_key in gauges: + result += get_influxdb_line(f"gauge.{gauge_key}", gauges[gauge_key], current_time, tags) + + # clear intermediate data + gauges = {} + last_flush_time = time.monotonic() + + # check that we aren't filling up the drive + if len(os.listdir(STATS_DIR)) < STATS_DIR_FILE_LIMIT: + if len(result) > 0: + stats_path = os.path.join(STATS_DIR, str(int(current_time.timestamp()))) + with atomic_write_in_dir(stats_path) as f: + f.write(result) + else: + cloudlog.error("stats dir full") + + +if __name__ == "__main__": + main() +else: + statlog = StatLog() diff --git a/selfdrive/test/helpers.py b/selfdrive/test/helpers.py index 0bb475c9296123..5abc0d964fb62a 100644 --- a/selfdrive/test/helpers.py +++ b/selfdrive/test/helpers.py @@ -13,7 +13,6 @@ def set_params_enabled(): params.put("HasAcceptedTerms", terms_version) params.put("CompletedTrainingVersion", training_version) params.put_bool("OpenpilotEnabledToggle", True) - params.put_bool("CommunityFeaturesToggle", True) params.put_bool("Passive", False) diff --git a/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py b/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py index 15a79b6ff683af..5021a1444d39a3 100755 --- a/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py +++ b/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py @@ -121,7 +121,6 @@ def setUpClass(cls): params.clear_all() params.put_bool("Passive", bool(os.getenv("PASSIVE"))) params.put_bool("OpenpilotEnabledToggle", True) - params.put_bool("CommunityFeaturesToggle", True) # hack def test_longitudinal_setup(self): diff --git a/selfdrive/test/process_replay/compare_logs.py b/selfdrive/test/process_replay/compare_logs.py index 72f5e51bb2419d..de734e52fb988b 100755 --- a/selfdrive/test/process_replay/compare_logs.py +++ b/selfdrive/test/process_replay/compare_logs.py @@ -2,6 +2,7 @@ import bz2 import os import sys +import math import numbers import dictdiffer from collections import Counter @@ -82,11 +83,16 @@ def compare_logs(log1, log2, ignore_fields=None, ignore_msgs=None, tolerance=Non dd = dictdiffer.diff(msg1_dict, msg2_dict, ignore=ignore_fields) # Dictdiffer only supports relative tolerance, we also want to check for absolute + # TODO: add this to dictdiffer def outside_tolerance(diff): - if diff[0] == "change": - a, b = diff[2] - if isinstance(a, numbers.Number) and isinstance(b, numbers.Number): - return abs(a - b) > max(tolerance, tolerance * max(abs(a), abs(b))) + try: + if diff[0] == "change": + a, b = diff[2] + finite = math.isfinite(a) and math.isfinite(b) + if finite and isinstance(a, numbers.Number) and isinstance(b, numbers.Number): + return abs(a - b) > max(tolerance, tolerance * max(abs(a), abs(b))) + except TypeError: + pass return True dd = list(filter(outside_tolerance, dd)) diff --git a/selfdrive/test/process_replay/process_replay.py b/selfdrive/test/process_replay/process_replay.py index 4b8442fc8dbff2..4ce3dd68a3ff01 100755 --- a/selfdrive/test/process_replay/process_replay.py +++ b/selfdrive/test/process_replay/process_replay.py @@ -25,7 +25,7 @@ CI = "CI" in os.environ TIMEOUT = 15 -ProcessConfig = namedtuple('ProcessConfig', ['proc_name', 'pub_sub', 'ignore', 'init_callback', 'should_recv_callback', 'tolerance', 'fake_pubsubmaster']) +ProcessConfig = namedtuple('ProcessConfig', ['proc_name', 'pub_sub', 'ignore', 'init_callback', 'should_recv_callback', 'tolerance', 'fake_pubsubmaster', 'submaster_config'], defaults=({},)) def wait_for_event(evt): @@ -88,8 +88,8 @@ def send(self, dat): class FakeSubMaster(messaging.SubMaster): - def __init__(self, services): - super().__init__(services, addr=None) + def __init__(self, services, ignore_alive=None, ignore_avg_freq=None): + super().__init__(services, ignore_alive=ignore_alive, ignore_avg_freq=ignore_avg_freq, addr=None) self.sock = {s: DumbSocket(s) for s in services} self.update_called = threading.Event() self.update_ready = threading.Event() @@ -172,7 +172,6 @@ def fingerprint(msgs, fsm, can_sock, fingerprint): can_sock.data = [] fsm.update_ready.set() - print("finished fingerprinting") def get_car_params(msgs, fsm, can_sock, fingerprint): @@ -241,13 +240,14 @@ def ublox_rcv_callback(msg): pub_sub={ "can": ["controlsState", "carState", "carControl", "sendcan", "carEvents", "carParams"], "deviceState": [], "pandaStates": [], "peripheralState": [], "liveCalibration": [], "driverMonitoringState": [], "longitudinalPlan": [], "lateralPlan": [], "liveLocationKalman": [], "liveParameters": [], "radarState": [], - "modelV2": [], "driverCameraState": [], "roadCameraState": [], "ubloxRaw": [], "managerState": [], + "modelV2": [], "driverCameraState": [], "roadCameraState": [], "managerState": [], }, ignore=["logMonoTime", "valid", "controlsState.startMonoTime", "controlsState.cumLagMs"], init_callback=fingerprint, should_recv_callback=controlsd_rcv_callback, tolerance=NUMPY_TOLERANCE, fake_pubsubmaster=True, + submaster_config={'ignore_avg_freq': ['radarState', 'longitudinalPlan']} ), ProcessConfig( proc_name="radard", @@ -267,7 +267,7 @@ def ublox_rcv_callback(msg): "modelV2": ["lateralPlan", "longitudinalPlan"], "carState": [], "controlsState": [], "radarState": [], }, - ignore=["logMonoTime", "valid", "longitudinalPlan.processingDelay"], + ignore=["logMonoTime", "valid", "longitudinalPlan.processingDelay", "longitudinalPlan.solverExecutionTime", "lateralPlan.solverExecutionTime"], init_callback=get_car_params, should_recv_callback=None, tolerance=NUMPY_TOLERANCE, @@ -341,21 +341,25 @@ def replay_process(cfg, lr, fingerprint=None): else: return cpp_replay_process(cfg, lr, fingerprint) -def setup_env(): +def setup_env(simulation=False): params = Params() params.clear_all() params.put_bool("OpenpilotEnabledToggle", True) params.put_bool("Passive", False) - params.put_bool("CommunityFeaturesToggle", True) - os.environ['NO_RADAR_SLEEP'] = "1" - os.environ["SIMULATION"] = "1" + os.environ["NO_RADAR_SLEEP"] = "1" + os.environ["REPLAY"] = "1" + + if simulation: + os.environ["SIMULATION"] = "1" + elif "SIMULATION" in os.environ: + del os.environ["SIMULATION"] def python_replay_process(cfg, lr, fingerprint=None): sub_sockets = [s for _, sub in cfg.pub_sub.items() for s in sub] pub_sockets = [s for s in cfg.pub_sub.keys() if s != 'can'] - fsm = FakeSubMaster(pub_sockets) + fsm = FakeSubMaster(pub_sockets, **cfg.submaster_config) fpm = FakePubMaster(sub_sockets) args = (fsm, fpm) if 'can' in list(cfg.pub_sub.keys()): @@ -426,7 +430,7 @@ def python_replay_process(cfg, lr, fingerprint=None): msg_queue.append(msg.as_builder()) if should_recv: - fsm.update_msgs(0, msg_queue) + fsm.update_msgs(msg.logMonoTime / 1e9, msg_queue) msg_queue = [] recv_cnt = len(recv_socks) @@ -448,7 +452,8 @@ def cpp_replay_process(cfg, lr, fingerprint=None): pub_msgs = [msg for msg in all_msgs if msg.which() in list(cfg.pub_sub.keys())] log_msgs = [] - setup_env() + # We need to fake SubMaster alive since we can't inject a fake clock + setup_env(simulation=True) managed_processes[cfg.proc_name].prepare() managed_processes[cfg.proc_name].start() diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 6ebdb327b302a7..9b81f7aeaa355a 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -05ebb83207d2c949ee70702e4ec4568f872c6054 \ No newline at end of file +602f92fc0e337ee5143dd2691f20fead68365b64 \ No newline at end of file diff --git a/selfdrive/test/process_replay/regen.py b/selfdrive/test/process_replay/regen.py index 5326075240333a..16ee5e9a283e72 100755 --- a/selfdrive/test/process_replay/regen.py +++ b/selfdrive/test/process_replay/regen.py @@ -11,8 +11,8 @@ from cereal.services import service_list from cereal.visionipc.visionipc_pyx import VisionIpcServer, VisionStreamType # pylint: disable=no-name-in-module, import-error from common.params import Params -from common.realtime import Ratekeeper, DT_MDL, DT_DMON -from common.transformations.camera import eon_f_frame_size, eon_d_frame_size +from common.realtime import Ratekeeper, DT_MDL, DT_DMON, sec_since_boot +from common.transformations.camera import eon_f_frame_size, eon_d_frame_size, tici_f_frame_size, tici_d_frame_size from selfdrive.car.fingerprints import FW_VERSIONS from selfdrive.manager.process import ensure_running from selfdrive.manager.process_config import managed_processes @@ -26,32 +26,107 @@ FAKEDATA = os.path.join(process_replay_dir, "fakedata/") +def replay_panda_states(s, msgs): + pm = messaging.PubMaster([s, 'peripheralState']) + rk = Ratekeeper(service_list[s].frequency, print_delay_threshold=None) + smsgs = [m for m in msgs if m.which() in ['pandaStates', 'pandaStateDEPRECATED']] + + # Migrate safety param base on carState + cp = [m for m in msgs if m.which() == 'carParams'][0].carParams + if len(cp.safetyConfigs): + safety_param = cp.safetyConfigs[0].safetyParam + else: + safety_param = cp.safetyParamDEPRECATED + + while True: + for m in smsgs: + if m.which() == 'pandaStateDEPRECATED': + new_m = messaging.new_message('pandaStates', 1) + new_m.pandaStates[0] = m.pandaStateDEPRECATED + new_m.pandaStates[0].safetyParam = safety_param + pm.send(s, new_m) + else: + new_m = m.as_builder() + new_m.logMonoTime = int(sec_since_boot() * 1e9) + pm.send(s, new_m) + + new_m = messaging.new_message('peripheralState') + pm.send('peripheralState', new_m) + + rk.keep_time() + + +def replay_manager_state(s, msgs): + pm = messaging.PubMaster([s, ]) + rk = Ratekeeper(service_list[s].frequency, print_delay_threshold=None) + + while True: + new_m = messaging.new_message('managerState') + new_m.managerState.processes = [{'name': name, 'running': True} for name in managed_processes] + pm.send(s, new_m) + rk.keep_time() + + +def replay_device_state(s, msgs): + pm = messaging.PubMaster([s, ]) + rk = Ratekeeper(service_list[s].frequency, print_delay_threshold=None) + smsgs = [m for m in msgs if m.which() == s] + while True: + for m in smsgs: + new_m = m.as_builder() + new_m.logMonoTime = int(sec_since_boot() * 1e9) + new_m.deviceState.freeSpacePercent = 50 + new_m.deviceState.memoryUsagePercent = 50 + pm.send(s, new_m) + rk.keep_time() + + +def replay_sensor_events(s, msgs): + pm = messaging.PubMaster([s, ]) + rk = Ratekeeper(service_list[s].frequency, print_delay_threshold=None) + smsgs = [m for m in msgs if m.which() == s] + while True: + for m in smsgs: + new_m = m.as_builder() + new_m.logMonoTime = int(sec_since_boot() * 1e9) + + for evt in new_m.sensorEvents: + evt.timestamp = new_m.logMonoTime + + pm.send(s, new_m) + rk.keep_time() + + def replay_service(s, msgs): pm = messaging.PubMaster([s, ]) rk = Ratekeeper(service_list[s].frequency, print_delay_threshold=None) smsgs = [m for m in msgs if m.which() == s] while True: for m in smsgs: - # TODO: use logMonoTime - pm.send(s, m.as_builder()) + new_m = m.as_builder() + new_m.logMonoTime = int(sec_since_boot() * 1e9) + pm.send(s, new_m) rk.keep_time() -vs = None + def replay_cameras(lr, frs): - cameras = [ + eon_cameras = [ ("roadCameraState", DT_MDL, eon_f_frame_size, VisionStreamType.VISION_STREAM_ROAD), ("driverCameraState", DT_DMON, eon_d_frame_size, VisionStreamType.VISION_STREAM_DRIVER), ] + tici_cameras = [ + ("roadCameraState", DT_MDL, tici_f_frame_size, VisionStreamType.VISION_STREAM_ROAD), + ("driverCameraState", DT_MDL, tici_d_frame_size, VisionStreamType.VISION_STREAM_DRIVER), + ] - def replay_camera(s, stream, dt, vipc_server, fr, size): + def replay_camera(s, stream, dt, vipc_server, frames, size): pm = messaging.PubMaster([s, ]) rk = Ratekeeper(1 / dt, print_delay_threshold=None) img = b"\x00" * int(size[0]*size[1]*3/2) while True: - if fr is not None: - img = fr.get(rk.frame % fr.frame_count, pix_fmt='yuv420p')[0] - img = img.flatten().tobytes() + if frames is not None: + img = frames[rk.frame % len(frames)] rk.keep_time() @@ -62,24 +137,34 @@ def replay_camera(s, stream, dt, vipc_server, fr, size): vipc_server.send(stream, img, msg.frameId, msg.timestampSof, msg.timestampEof) + init_data = [m for m in lr if m.which() == 'initData'][0] + cameras = tici_cameras if (init_data.initData.deviceType == 'tici') else eon_cameras + # init vipc server and cameras p = [] - global vs vs = VisionIpcServer("camerad") for (s, dt, size, stream) in cameras: fr = frs.get(s, None) + + frames = None + if fr is not None: + print(f"Decomressing frames {s}") + frames = [] + for i in tqdm(range(fr.frame_count)): + img = fr.get(i, pix_fmt='yuv420p')[0] + frames.append(img.flatten().tobytes()) + vs.create_buffers(stream, 40, False, size[0], size[1]) p.append(multiprocessing.Process(target=replay_camera, - args=(s, stream, dt, vs, fr, size))) + args=(s, stream, dt, vs, frames, size))) # hack to make UI work vs.create_buffers(VisionStreamType.VISION_STREAM_RGB_BACK, 4, True, eon_f_frame_size[0], eon_f_frame_size[1]) vs.start_listener() - return p + return vs, p def regen_segment(lr, frs=None, outdir=FAKEDATA): - lr = list(lr) if frs is None: frs = dict() @@ -89,53 +174,48 @@ def regen_segment(lr, frs=None, outdir=FAKEDATA): params.clear_all() params.put_bool("Passive", False) params.put_bool("OpenpilotEnabledToggle", True) - params.put_bool("CommunityFeaturesToggle", True) - params.put_bool("CommunityFeaturesToggle", True) - cal = messaging.new_message('liveCalibration') - cal.liveCalibration.validBlocks = 20 - cal.liveCalibration.rpyCalib = [0.0, 0.0, 0.0] - params.put("CalibrationParams", cal.to_bytes()) os.environ["LOG_ROOT"] = outdir - os.environ["SIMULATION"] = "1" + os.environ["REPLAY"] = "1" os.environ['SKIP_FW_QUERY'] = "" os.environ['FINGERPRINT'] = "" + + # TODO: remove after getting new route for mazda + migration = { + "Mazda CX-9 2021": "MAZDA CX-9 2021", + } + for msg in lr: if msg.which() == 'carParams': - car_fingerprint = msg.carParams.carFingerprint + car_fingerprint = migration.get(msg.carParams.carFingerprint, msg.carParams.carFingerprint) if len(msg.carParams.carFw) and (car_fingerprint in FW_VERSIONS): params.put("CarParamsCache", msg.carParams.as_builder().to_bytes()) else: os.environ['SKIP_FW_QUERY'] = "1" os.environ['FINGERPRINT'] = car_fingerprint + elif msg.which() == 'liveCalibration': + params.put("CalibrationParams", msg.as_builder().to_bytes()) - #TODO: init car, make sure starts engaged when segment is engaged + vs, cam_procs = replay_cameras(lr, frs) fake_daemons = { 'sensord': [ - multiprocessing.Process(target=replay_service, args=('sensorEvents', lr)), + multiprocessing.Process(target=replay_sensor_events, args=('sensorEvents', lr)), ], 'pandad': [ multiprocessing.Process(target=replay_service, args=('can', lr)), - multiprocessing.Process(target=replay_service, args=('pandaStates', lr)), + multiprocessing.Process(target=replay_service, args=('ubloxRaw', lr)), + multiprocessing.Process(target=replay_panda_states, args=('pandaStates', lr)), + ], + 'managerState': [ + multiprocessing.Process(target=replay_manager_state, args=('managerState', lr)), ], - #'managerState': [ - # multiprocessing.Process(target=replay_service, args=('managerState', lr)), - #], 'thermald': [ - multiprocessing.Process(target=replay_service, args=('deviceState', lr)), + multiprocessing.Process(target=replay_device_state, args=('deviceState', lr)), ], 'camerad': [ - *replay_cameras(lr, frs), - ], - - # TODO: fix these and run them - 'paramsd': [ - multiprocessing.Process(target=replay_service, args=('liveParameters', lr)), - ], - 'locationd': [ - multiprocessing.Process(target=replay_service, args=('liveLocationKalman', lr)), + *cam_procs, ], } @@ -162,11 +242,13 @@ def regen_segment(lr, frs=None, outdir=FAKEDATA): for p in procs: p.terminate() + del vs + r = params.get("CurrentRoute", encoding='utf-8') return os.path.join(outdir, r + "--0") -def regen_and_save(route, sidx, upload=False, use_route_meta=True): +def regen_and_save(route, sidx, upload=False, use_route_meta=False): if use_route_meta: r = Route(args.route) lr = LogReader(r.log_paths()[args.seg]) @@ -175,6 +257,11 @@ def regen_and_save(route, sidx, upload=False, use_route_meta=True): lr = LogReader(f"cd:/{route.replace('|', '/')}/{sidx}/rlog.bz2") fr = FrameReader(f"cd:/{route.replace('|', '/')}/{sidx}/fcamera.hevc") rpath = regen_segment(lr, {'roadCameraState': fr}) + + lr = LogReader(os.path.join(rpath, 'rlog.bz2')) + controls_state_active = [m.controlsState.active for m in lr if m.which() == 'controlsState'] + assert any(controls_state_active), "Segment did not engage" + relr = os.path.relpath(rpath) print("\n\n", "*"*30, "\n\n") diff --git a/selfdrive/test/process_replay/regen_all.py b/selfdrive/test/process_replay/regen_all.py old mode 100644 new mode 100755 index 047c83f4658f18..4f057bbf50c922 --- a/selfdrive/test/process_replay/regen_all.py +++ b/selfdrive/test/process_replay/regen_all.py @@ -7,12 +7,13 @@ for segment in segments: route = segment[1].rsplit('--', 1)[0] sidx = int(segment[1].rsplit('--', 1)[1]) + print("Regen", route, sidx) relr = regen_and_save(route, sidx, upload=True, use_route_meta=False) print("\n\n", "*"*30, "\n\n") print("New route:", relr, "\n") relr = relr.replace('/', '|') - new_segments.append(f'("{segment[0]}", "{relr}"), ') + new_segments.append(f' ("{segment[0]}", "{relr}"), ') print() print() print() diff --git a/selfdrive/test/process_replay/test_processes.py b/selfdrive/test/process_replay/test_processes.py index 6c33b46224569a..f7dae5c9fbd7de 100755 --- a/selfdrive/test/process_replay/test_processes.py +++ b/selfdrive/test/process_replay/test_processes.py @@ -30,18 +30,18 @@ ] segments = [ - ("HYUNDAI", "fakedata|2021-10-07--15-56-26--0"), - ("TOYOTA", "fakedata|2021-10-07--15-57-47--0"), - ("TOYOTA2", "fakedata|2021-10-07--15-59-03--0"), - ("TOYOTA3", "fakedata|2021-10-07--15-53-21--0"), - ("HONDA", "fakedata|2021-10-07--16-00-19--0"), - ("HONDA2", "fakedata|2021-10-07--16-01-35--0"), - ("CHRYSLER", "fakedata|2021-10-07--16-02-52--0"), - ("SUBARU", "fakedata|2021-10-07--16-04-09--0"), - ("GM", "fakedata|2021-10-07--16-05-26--0"), - ("NISSAN", "fakedata|2021-10-07--16-09-53--0"), - ("VOLKSWAGEN", "fakedata|2021-10-07--16-11-11--0"), - ("MAZDA", "bd6a637565e91581|2021-10-30--15-14-53--2"), + ("HYUNDAI", "fakedata|2022-01-20--17-49-04--0"), + ("TOYOTA", "fakedata|2022-01-20--17-50-51--0"), + ("TOYOTA2", "fakedata|2022-01-20--17-52-36--0"), + ("TOYOTA3", "fakedata|2022-01-20--17-54-50--0"), + ("HONDA", "fakedata|2022-01-20--17-56-40--0"), + ("HONDA2", "fakedata|2022-01-20--17-58-25--0"), + ("CHRYSLER", "fakedata|2022-01-20--18-00-11--0"), + ("SUBARU", "fakedata|2022-01-20--18-01-57--0"), + ("GM", "fakedata|2022-01-20--18-03-41--0"), + ("NISSAN", "fakedata|2022-01-20--18-05-29--0"), + ("VOLKSWAGEN", "fakedata|2022-01-20--18-07-15--0"), + ("MAZDA", "fakedata|2022-01-20--18-09-32--0"), ] # dashcamOnly makes don't need to be tested until a full port is done @@ -65,9 +65,7 @@ def test_process(cfg, lr, cmp_log_fn, ignore_fields=None, ignore_msgs=None): log_msgs = replay_process(cfg, lr) # check to make sure openpilot is engaged in the route - # TODO: update routes so enable check can run - # failed enable check: honda bosch, hyundai, chrysler, and subaru - if cfg.proc_name == "controlsd" and FULL_TEST and False: + if cfg.proc_name == "controlsd": for msg in log_msgs: if msg.which() == "controlsState": if msg.controlsState.active: diff --git a/selfdrive/test/test_fingerprints.py b/selfdrive/test/test_fingerprints.py index 4ea60a76a8cc58..01c7d753c3136a 100755 --- a/selfdrive/test/test_fingerprints.py +++ b/selfdrive/test/test_fingerprints.py @@ -1,6 +1,8 @@ #!/usr/bin/env python3 import os import sys +from typing import Dict, List + from common.basedir import BASEDIR # messages reserved for CAN based ignition (see can_ignition_hook function in panda/board/drivers/can) @@ -62,7 +64,7 @@ def check_can_ignition_conflicts(fingerprints, brands): if __name__ == "__main__": fingerprints = _get_fingerprints() - fingerprints_flat = [] + fingerprints_flat: List[Dict] = [] car_names = [] brand_names = [] for brand in fingerprints: diff --git a/selfdrive/test/test_models.py b/selfdrive/test/test_models.py index 823d7706ddd844..331bfe917f954f 100755 --- a/selfdrive/test/test_models.py +++ b/selfdrive/test/test_models.py @@ -4,16 +4,19 @@ import importlib import unittest from collections import defaultdict, Counter +from typing import List, Optional, Tuple from parameterized import parameterized_class from cereal import log, car from common.params import Params +from common.realtime import DT_CTRL +from selfdrive.boardd.boardd import can_capnp_to_can_list, can_list_to_can_capnp from selfdrive.car.fingerprints import all_known_cars from selfdrive.car.car_helpers import interfaces from selfdrive.car.gm.values import CAR as GM -from selfdrive.car.honda.values import HONDA_BOSCH, CAR as HONDA -from selfdrive.car.chrysler.values import CAR as CHRYSLER +from selfdrive.car.honda.values import CAR as HONDA from selfdrive.car.hyundai.values import CAR as HYUNDAI +from selfdrive.car.toyota.values import CAR as TOYOTA from selfdrive.test.test_routes import routes, non_tested_cars from selfdrive.test.openpilotci import get_url from tools.lib.logreader import LogReader @@ -26,42 +29,44 @@ NUM_JOBS = int(os.environ.get("NUM_JOBS", "1")) JOB_ID = int(os.environ.get("JOB_ID", "0")) -ROUTES = {rt.car_fingerprint: rt.route for rt in routes} - # TODO: get updated routes for these cars ignore_can_valid = [ HYUNDAI.SANTA_FE, ] -ignore_carstate_check = [ - # TODO: chrysler gas state in panda also checks wheel speed, refactor so it's only gas - CHRYSLER.PACIFICA_2017_HYBRID, -] - ignore_addr_checks_valid = [ GM.BUICK_REGAL, HYUNDAI.GENESIS_G70_2020, ] -@parameterized_class(('car_model'), [(car,) for i, car in enumerate(sorted(all_known_cars())) if i % NUM_JOBS == JOB_ID]) +# build list of test cases +routes_by_car = defaultdict(set) +for r in routes: + routes_by_car[r.car_fingerprint].add(r.route) + +test_cases: List[Tuple[str, Optional[str]]] = [] +for i, c in enumerate(sorted(all_known_cars())): + if i % NUM_JOBS == JOB_ID: + test_cases.extend((c, r) for r in routes_by_car.get(c, (None, ))) + + +@parameterized_class(('car_model', 'route'), test_cases) class TestCarModel(unittest.TestCase): @classmethod def setUpClass(cls): - if cls.car_model not in ROUTES: - # TODO: get routes for missing cars and remove this + if cls.route is None: if cls.car_model in non_tested_cars: print(f"Skipping tests for {cls.car_model}: missing route") raise unittest.SkipTest - else: - raise Exception(f"missing test route for car {cls.car_model}") + raise Exception(f"missing test route for {cls.car_model}") params = Params() params.clear_all() for seg in [2, 1, 0]: try: - lr = LogReader(get_url(ROUTES[cls.car_model], seg)) + lr = LogReader(get_url(cls.route, seg)) except Exception: continue @@ -77,16 +82,17 @@ def setUpClass(cls): if msg.carParams.openpilotLongitudinalControl: params.put_bool("DisableRadar", True) - if len(can_msgs): + if len(can_msgs) > int(50 / DT_CTRL): break else: - raise Exception("Route not found or no CAN msgs found. Is it uploaded?") + raise Exception(f"Route {repr(cls.route)} not found or no CAN msgs found. Is it uploaded?") cls.can_msgs = sorted(can_msgs, key=lambda msg: msg.logMonoTime) cls.CarInterface, cls.CarController, cls.CarState = interfaces[cls.car_model] cls.CP = cls.CarInterface.get_params(cls.car_model, fingerprint, []) assert cls.CP + assert cls.CP.carFingerprint == cls.car_model def setUp(self): self.CI = self.CarInterface(self.CP, self.CarController, self.CarState) @@ -181,53 +187,57 @@ def test_panda_safety_carstate(self): """ if self.CP.dashcamOnly: self.skipTest("no need to check panda safety for dashcamOnly") - if self.car_model in ignore_carstate_check: - self.skipTest("see comments in test_models.py") - checks = defaultdict(lambda: 0) CC = car.CarControl.new_message() + + # warm up pass, as initial states may be different + for can in self.can_msgs[:300]: + for msg in can_capnp_to_can_list(can.can, src_filter=range(64)): + to_send = package_can_msg(msg) + self.safety.safety_rx_hook(to_send) + self.CI.update(CC, (can_list_to_can_capnp([msg, ]), )) + + checks = defaultdict(lambda: 0) for can in self.can_msgs: - for msg in can.can: - if msg.src >= 64: - continue - to_send = package_can_msg([msg.address, 0, msg.dat, msg.src]) + for msg in can_capnp_to_can_list(can.can, src_filter=range(64)): + to_send = package_can_msg(msg) ret = self.safety.safety_rx_hook(to_send) self.assertEqual(1, ret, f"safety rx failed ({ret=}): {to_send}") - CS = self.CI.update(CC, (can.as_builder().to_bytes(),)) + CS = self.CI.update(CC, (can_list_to_can_capnp([msg, ]), )) + + # TODO: check rest of panda's carstate (steering, ACC main on, etc.) + + # TODO: make the interceptor thresholds in openpilot and panda match, then remove this exception + gas_pressed = CS.gasPressed + if self.CP.enableGasInterceptor and gas_pressed and not self.safety.get_gas_pressed_prev(): + # panda intentionally has a higher threshold + if self.CP.carName == "toyota" and 15 < CS.gas < 15*1.5: + gas_pressed = False + if self.CP.carName == "honda": + gas_pressed = False + checks['gasPressed'] += gas_pressed != self.safety.get_gas_pressed_prev() + + # TODO: remove this exception once this mismatch is resolved + brake_pressed = CS.brakePressed + if CS.brakePressed and not self.safety.get_brake_pressed_prev(): + if self.CP.carFingerprint in (HONDA.PILOT, HONDA.PASSPORT, HONDA.RIDGELINE) and CS.brake > 0.05: + brake_pressed = False + if self.CP.carName == "honda" and self.CI.CS.brake_switch: + brake_pressed = False + checks['brakePressed'] += brake_pressed != self.safety.get_brake_pressed_prev() - # TODO: check steering state - # check that openpilot and panda safety agree on the car's state - checks['gasPressed'] += CS.gasPressed != self.safety.get_gas_pressed_prev() - checks['brakePressed'] += CS.brakePressed != self.safety.get_brake_pressed_prev() if self.CP.pcmCruise: checks['controlsAllowed'] += not CS.cruiseState.enabled and self.safety.get_controls_allowed() - # TODO: extend this to all cars if self.CP.carName == "honda": checks['mainOn'] += CS.cruiseState.available != self.safety.get_acc_main_on() - # TODO: reduce tolerance to 0 - failed_checks = {k: v for k, v in checks.items() if v > 25} - - # TODO: the panda and openpilot interceptor thresholds should match - skip_gas_check = self.CP.carName == 'chrysler' - if "gasPressed" in failed_checks and (self.CP.enableGasInterceptor or skip_gas_check): - if failed_checks['gasPressed'] < 150 or skip_gas_check: - del failed_checks['gasPressed'] - - # TODO: honda nidec: do same checks in carState and panda - if "brakePressed" in failed_checks and self.CP.carName == 'honda' and \ - (self.car_model not in HONDA_BOSCH or self.car_model in [HONDA.CRV_HYBRID, HONDA.HONDA_E]): - if failed_checks['brakePressed'] < 150: - del failed_checks['brakePressed'] - - # TODO: use the same signal in panda and carState - # tolerate a small delay between the button press and PCM entering a cruise state - if self.car_model == HONDA.ACCORD: - if failed_checks['controlsAllowed'] < 500: - del failed_checks['controlsAllowed'] + # TODO: add flag to toyota safety + if self.CP.carFingerprint == TOYOTA.SIENNA and checks['brakePressed'] < 25: + checks['brakePressed'] = 0 - self.assertFalse(len(failed_checks), f"panda safety doesn't agree with CarState: {failed_checks}") + failed_checks = {k: v for k, v in checks.items() if v > 0} + self.assertFalse(len(failed_checks), f"panda safety doesn't agree with openpilot: {failed_checks}") if __name__ == "__main__": unittest.main() diff --git a/selfdrive/test/test_onroad.py b/selfdrive/test/test_onroad.py index 36e8c5499f0db5..1136bc6b2670fa 100755 --- a/selfdrive/test/test_onroad.py +++ b/selfdrive/test/test_onroad.py @@ -34,7 +34,7 @@ "./_modeld": 4.48, "./boardd": 3.63, "./_dmonitoringmodeld": 2.67, - "selfdrive.thermald.thermald": 2.41, + "selfdrive.thermald.thermald": 5.36, "selfdrive.locationd.calibrationd": 2.0, "./_soundd": 1.0, "selfdrive.monitoring.dmonitoringd": 1.90, @@ -61,7 +61,7 @@ "./_dmonitoringmodeld": 10.0, "selfdrive.locationd.paramsd": 5.0, "selfdrive.controls.radard": 3.6, - "selfdrive.thermald.thermald": 1.5, + "selfdrive.thermald.thermald": 3.87, }) @@ -204,6 +204,22 @@ def test_cpu_usage(self): cpu_ok = check_cpu_usage(proclogs[0], proclogs[-1]) self.assertTrue(cpu_ok) + def test_mpc_execution_timings(self): + result = "\n" + result += "------------------------------------------------\n" + result += "----------------- MPC Timing ------------------\n" + result += "------------------------------------------------\n" + + cfgs = [("lateralPlan", 0.05, 0.05), ("longitudinalPlan", 0.05, 0.05)] + for (s, instant_max, avg_max) in cfgs: + ts = [getattr(getattr(m, s), "solverExecutionTime") for m in self.lr if m.which() == s] + self.assertLess(min(ts), instant_max, f"high '{s}' execution time: {min(ts)}") + self.assertLess(np.mean(ts), avg_max, f"high avg '{s}' execution time: {np.mean(ts)}") + result += f"'{s}' execution time: {min(ts)}\n" + result += f"'{s}' avg execution time: {np.mean(ts)}\n" + result += "------------------------------------------------\n" + print(result) + def test_model_execution_timings(self): result = "\n" result += "------------------------------------------------\n" diff --git a/selfdrive/test/test_routes.py b/selfdrive/test/test_routes.py old mode 100755 new mode 100644 index 053d2d4bfbd9dc..40404319203a6c --- a/selfdrive/test/test_routes.py +++ b/selfdrive/test/test_routes.py @@ -2,7 +2,6 @@ from collections import namedtuple from selfdrive.car.chrysler.values import CAR as CHRYSLER -from selfdrive.car.ford.values import CAR as FORD from selfdrive.car.gm.values import CAR as GM from selfdrive.car.honda.values import CAR as HONDA from selfdrive.car.hyundai.values import CAR as HYUNDAI @@ -34,7 +33,7 @@ TestRoute("8190c7275a24557b|2020-01-29--08-33-58", CHRYSLER.PACIFICA_2019_HYBRID), TestRoute("3d84727705fecd04|2021-05-25--08-38-56", CHRYSLER.PACIFICA_2020), - TestRoute("f1b4c567731f4a1b|2018-04-30--10-15-35", FORD.FUSION), + #TestRoute("f1b4c567731f4a1b|2018-04-30--10-15-35", FORD.FUSION), TestRoute("7cc2a8365b4dd8a9|2018-12-02--12-10-44", GM.ACADIA), TestRoute("aa20e335f61ba898|2019-02-05--16-59-04", GM.BUICK_REGAL), @@ -61,7 +60,6 @@ TestRoute("d83f36766f8012a5|2020-02-05--18-42-21", HONDA.CIVIC_BOSCH_DIESEL), TestRoute("f0890d16a07a236b|2021-05-25--17-27-22", HONDA.INSIGHT), TestRoute("07d37d27996096b6|2020-03-04--21-57-27", HONDA.PILOT), - TestRoute("fa1cd231131ca137|2021-05-22--07-59-57", HONDA.PILOT_2019), TestRoute("684e8f96bd491a0e|2021-11-03--11-08-42", HONDA.PASSPORT), TestRoute("0a78dfbacc8504ef|2020-03-04--13-29-55", HONDA.CIVIC_BOSCH), TestRoute("f34a60d68d83b1e5|2020-10-06--14-35-55", HONDA.ACURA_RDX), @@ -116,8 +114,6 @@ TestRoute("5f5afb36036506e4|2019-05-14--02-09-54", TOYOTA.COROLLA_TSS2), TestRoute("5ceff72287a5c86c|2019-10-19--10-59-02", TOYOTA.COROLLAH_TSS2), TestRoute("d2525c22173da58b|2021-04-25--16-47-04", TOYOTA.PRIUS), - TestRoute("b0f5a01cf604185c|2017-12-18--20-32-32", TOYOTA.RAV4), - TestRoute("b0c9d2329ad1606b|2019-04-02--13-24-43", TOYOTA.RAV4), TestRoute("b14c5b4742e6fc85|2020-07-28--19-50-11", TOYOTA.RAV4), TestRoute("32a7df20486b0f70|2020-02-06--16-06-50", TOYOTA.RAV4H), TestRoute("cdf2f7de565d40ae|2019-04-25--03-53-41", TOYOTA.RAV4_TSS2), @@ -145,6 +141,7 @@ TestRoute("9b36accae406390e|2021-03-30--10-41-38", TOYOTA.MIRAI), TestRoute("cd9cff4b0b26c435|2021-05-13--15-12-39", TOYOTA.CHR), TestRoute("57858ede0369a261|2021-05-18--20-34-20", TOYOTA.CHRH), + TestRoute("14623aae37e549f3|2021-10-24--01-20-49", TOYOTA.PRIUS_V), TestRoute("202c40641158a6e5|2021-09-21--09-43-24", VOLKSWAGEN.ARTEON_MK1), TestRoute("2c68dda277d887ac|2021-05-11--15-22-20", VOLKSWAGEN.ATLAS_MK1), @@ -198,19 +195,3 @@ TestRoute("6c14ee12b74823ce|2021-06-30--11-49-02", TESLA.AP1_MODELS), TestRoute("bb50caf5f0945ab1|2021-06-19--17-20-18", TESLA.AP2_MODELS), ] - -forced_dashcam_routes = [ - # Ford fusion - "f1b4c567731f4a1b|2018-04-18--11-29-37", - "f1b4c567731f4a1b|2018-04-30--10-15-35", - # Mazda CX5 - "32a319f057902bb3|2020-04-27--15-18-58", - # Mazda CX9 - "10b5a4b380434151|2020-08-26--17-11-45", - # Mazda3 - "74f1038827005090|2020-08-26--20-05-50", - # Mazda6 - "fb53c640f499b73d|2021-06-01--04-17-56", - # CX-9 2021 - "f6d5b1a9d7a1c92e|2021-07-08--06-56-59", -] diff --git a/selfdrive/thermald/power_monitoring.py b/selfdrive/thermald/power_monitoring.py index eb7f300caa3120..f1626f8cfc6153 100644 --- a/selfdrive/thermald/power_monitoring.py +++ b/selfdrive/thermald/power_monitoring.py @@ -9,6 +9,7 @@ from common.realtime import sec_since_boot from selfdrive.hardware import HARDWARE from selfdrive.swaglog import cloudlog +from selfdrive.statsd import statlog from common.op_params import opParams CAR_VOLTAGE_LOW_PASS_K = 0.091 # LPF gain for 5s tau (dt/tau / (dt/tau + 1)) @@ -57,6 +58,7 @@ def calculate(self, peripheralState, ignition): # Low-pass battery voltage self.car_voltage_instant_mV = peripheralState.voltage self.car_voltage_mV = ((peripheralState.voltage * CAR_VOLTAGE_LOW_PASS_K) + (self.car_voltage_mV * (1 - CAR_VOLTAGE_LOW_PASS_K))) + statlog.gauge("car_voltage", self.car_voltage_mV / 1e3) # Cap the car battery power and save it in a param every 10-ish seconds self.car_battery_capacity_uWh = max(self.car_battery_capacity_uWh, 0) diff --git a/selfdrive/thermald/thermald.py b/selfdrive/thermald/thermald.py index f1cc947ee835af..e6b61d1ba05b11 100755 --- a/selfdrive/thermald/thermald.py +++ b/selfdrive/thermald/thermald.py @@ -1,25 +1,28 @@ #!/usr/bin/env python3 import datetime import os +import queue +import threading import time +from collections import OrderedDict, namedtuple from pathlib import Path -from typing import Dict, NoReturn, Optional, Tuple -from collections import namedtuple, OrderedDict +from typing import Dict, Optional, Tuple import psutil from smbus2 import SMBus import cereal.messaging as messaging from cereal import log +from common.dict_helpers import strip_deprecated_keys from common.filter_simple import FirstOrderFilter from common.numpy_fast import interp -from common.params import Params, ParamKeyType +from common.params import Params from common.realtime import DT_TRML, sec_since_boot -from common.dict_helpers import strip_deprecated_keys from selfdrive.controls.lib.alertmanager import set_offroad_alert from selfdrive.controls.lib.pid import PIDController -from selfdrive.hardware import EON, TICI, PC, HARDWARE +from selfdrive.hardware import EON, HARDWARE, PC, TICI from selfdrive.loggerd.config import get_available_percent +from selfdrive.statsd import statlog from selfdrive.swaglog import cloudlog from selfdrive.thermald.power_monitoring import PowerMonitoring from selfdrive.version import terms_version, training_version @@ -30,8 +33,10 @@ CURRENT_TAU = 15. # 15s time constant TEMP_TAU = 5. # 5s time constant DISCONNECT_TIMEOUT = 5. # wait 5 seconds before going offroad after disconnect so you get an alert +PANDA_STATES_TIMEOUT = int(1000 * 2.5 * DT_TRML) # 2.5x the expected pandaState frequency ThermalBand = namedtuple("ThermalBand", ['min_temp', 'max_temp']) +HardwareState = namedtuple("HardwareState", ['network_type', 'network_strength', 'network_info', 'nvme_temps', 'modem_temps']) # List of thermal bands. We will stay within this region as long as we are within the bounds. # When exiting the bounds, we'll jump to the lower or higher band. Bands are ordered in the dict. @@ -152,13 +157,50 @@ def set_offroad_alert_if_changed(offroad_alert: str, show_alert: bool, extra_tex set_offroad_alert(offroad_alert, show_alert, extra_text) -def thermald_thread() -> NoReturn: +def hw_state_thread(end_event, hw_queue): + """Handles non critical hardware state, and sends over queue""" + count = 0 + registered_count = 0 - pm = messaging.PubMaster(['deviceState']) + while not end_event.is_set(): + # these are expensive calls. update every 10s + if (count % int(10. / DT_TRML)) == 0: + try: + network_type = HARDWARE.get_network_type() + + hw_state = HardwareState( + network_type=network_type, + network_strength=HARDWARE.get_network_strength(network_type), + network_info=HARDWARE.get_network_info(), + nvme_temps=HARDWARE.get_nvme_temperatures(), + modem_temps=HARDWARE.get_modem_temperatures(), + ) - pandaState_timeout = int(1000 * 2.5 * DT_TRML) # 2.5x the expected pandaState frequency - pandaState_sock = messaging.sub_sock('pandaStates', timeout=pandaState_timeout) - sm = messaging.SubMaster(["peripheralState", "gpsLocationExternal", "managerState", "sentryState"]) + try: + hw_queue.put_nowait(hw_state) + except queue.Full: + pass + + if TICI and (hw_state.network_info is not None) and (hw_state.network_info.get('state', None) == "REGISTERED"): + registered_count += 1 + else: + registered_count = 0 + + if registered_count > 10: + cloudlog.warning(f"Modem stuck in registered state {hw_state.network_info}. nmcli conn up lte") + os.system("nmcli conn up lte") + registered_count = 0 + + except Exception: + cloudlog.exception("Error getting network status") + + count += 1 + time.sleep(DT_TRML) + + +def thermald_thread(end_event, hw_queue): + pm = messaging.PubMaster(['deviceState']) + sm = messaging.SubMaster(["peripheralState", "gpsLocationExternal", "controlsState", "pandaStates", "sentryState"], poll=["pandaStates"]) fan_speed = 0 count = 0 @@ -175,57 +217,43 @@ def thermald_thread() -> NoReturn: thermal_status = ThermalStatus.green usb_power = True - network_type = NetworkType.none - network_strength = NetworkStrength.unknown - network_info = None - modem_version = None - registered_count = 0 - nvme_temps = None - modem_temps = None + last_hw_state = HardwareState( + network_type=NetworkType.none, + network_strength=NetworkStrength.unknown, + network_info=None, + nvme_temps=[], + modem_temps=[], + ) current_filter = FirstOrderFilter(0., CURRENT_TAU, DT_TRML) temp_filter = FirstOrderFilter(0., TEMP_TAU, DT_TRML) - pandaState_prev = None should_start_prev = False in_car = False handle_fan = None is_uno = False - ui_running_prev = False + engaged_prev = False params = Params() power_monitor = PowerMonitoring() - no_panda_cnt = 0 HARDWARE.initialize_hardware() thermal_config = HARDWARE.get_thermal_config() # TODO: use PI controller for UNO - controller = PIDController(k_p=0, k_i=2e-3, neg_limit=-80, pos_limit=0, rate=(1 / DT_TRML)) - - # Leave flag for loggerd to indicate device was left onroad - if params.get_bool("IsOnroad"): - params.put_bool("BootedOnroad", True) + controller = PIDController(k_p=0, k_i=2e-3, k_d=0, neg_limit=-80, pos_limit=0, rate=(1 / DT_TRML)) - while True: - pandaStates = messaging.recv_sock(pandaState_sock, wait=True) + while not end_event.is_set(): + sm.update(PANDA_STATES_TIMEOUT) - sm.update(0) + pandaStates = sm['pandaStates'] peripheralState = sm['peripheralState'] msg = read_thermal(thermal_config) - if pandaStates is not None and len(pandaStates.pandaStates) > 0: - pandaState = pandaStates.pandaStates[0] + if sm.updated['pandaStates'] and len(pandaStates) > 0: + pandaState = pandaStates[0] - # If we lose connection to the panda, wait 5 seconds before going offroad - if pandaState.pandaType == log.PandaState.PandaType.unknown: - no_panda_cnt += 1 - if no_panda_cnt > DISCONNECT_TIMEOUT / DT_TRML: - if onroad_conditions["ignition"]: - cloudlog.error("Lost panda connection while onroad") - onroad_conditions["ignition"] = False - else: - no_panda_cnt = 0 + if pandaState.pandaType != log.PandaState.PandaType.unknown: onroad_conditions["ignition"] = pandaState.ignitionLine or pandaState.ignitionCan in_car = pandaState.harnessStatus != log.PandaState.HarnessStatus.notConnected @@ -246,53 +274,23 @@ def thermald_thread() -> NoReturn: setup_eon_fan() handle_fan = handle_fan_eon - # Handle disconnect - if pandaState_prev is not None: - if pandaState.pandaType == log.PandaState.PandaType.unknown and \ - pandaState_prev.pandaType != log.PandaState.PandaType.unknown: - params.clear_all(ParamKeyType.CLEAR_ON_PANDA_DISCONNECT) - pandaState_prev = pandaState - - # these are expensive calls. update every 10s - if (count % int(10. / DT_TRML)) == 0: - try: - network_type = HARDWARE.get_network_type() - network_strength = HARDWARE.get_network_strength(network_type) - network_info = HARDWARE.get_network_info() # pylint: disable=assignment-from-none - nvme_temps = HARDWARE.get_nvme_temperatures() - modem_temps = HARDWARE.get_modem_temperatures() - - # Log modem version once - if modem_version is None: - modem_version = HARDWARE.get_modem_version() # pylint: disable=assignment-from-none - if modem_version is not None: - cloudlog.warning(f"Modem version: {modem_version}") - - if TICI and (network_info.get('state', None) == "REGISTERED"): - registered_count += 1 - else: - registered_count = 0 - - if registered_count > 10: - cloudlog.warning(f"Modem stuck in registered state {network_info}. nmcli conn up lte") - os.system("nmcli conn up lte") - registered_count = 0 - - except Exception: - cloudlog.exception("Error getting network status") + try: + last_hw_state = hw_queue.get_nowait() + except queue.Empty: + pass msg.deviceState.freeSpacePercent = get_available_percent(default=100.0) msg.deviceState.memoryUsagePercent = int(round(psutil.virtual_memory().percent)) msg.deviceState.cpuUsagePercent = [int(round(n)) for n in psutil.cpu_percent(percpu=True)] msg.deviceState.gpuUsagePercent = int(round(HARDWARE.get_gpu_usage_percent())) - msg.deviceState.networkType = network_type - msg.deviceState.networkStrength = network_strength - if network_info is not None: - msg.deviceState.networkInfo = network_info - if nvme_temps is not None: - msg.deviceState.nvmeTempC = nvme_temps - if modem_temps is not None: - msg.deviceState.modemTempC = modem_temps + + msg.deviceState.networkType = last_hw_state.network_type + msg.deviceState.networkStrength = last_hw_state.network_strength + if last_hw_state.network_info is not None: + msg.deviceState.networkInfo = last_hw_state.network_info + + msg.deviceState.nvmeTempC = last_hw_state.nvme_temps + msg.deviceState.modemTempC = last_hw_state.modem_temps msg.deviceState.screenBrightnessPercent = HARDWARE.get_screen_brightness() msg.deviceState.batteryPercent = HARDWARE.get_battery_capacity() @@ -344,7 +342,8 @@ def thermald_thread() -> NoReturn: set_offroad_alert_if_changed("Offroad_TemperatureTooHigh", (not onroad_conditions["device_temp_good"])) if TICI: - set_offroad_alert_if_changed("Offroad_StorageMissing", (not Path("/data/media").is_mount())) + missing = (not Path("/data/media").is_mount()) and (not os.path.isfile("/persist/comma/living-in-the-moment")) + set_offroad_alert_if_changed("Offroad_StorageMissing", missing) # Handle offroad/onroad transition should_start = all(onroad_conditions.values()) or sm["sentryState"].started @@ -355,8 +354,23 @@ def thermald_thread() -> NoReturn: if should_start != should_start_prev or (count == 0): params.put_bool("IsOnroad", should_start) params.put_bool("IsOffroad", not should_start) + + params.put_bool("IsEngaged", False) + engaged_prev = False HARDWARE.set_power_save(not should_start) + if sm.updated['controlsState']: + engaged = sm['controlsState'].enabled + if engaged != engaged_prev: + params.put_bool("IsEngaged", engaged) + engaged_prev = engaged + + try: + with open('/dev/kmsg', 'w') as kmsg: + kmsg.write(f"<3>[thermald] engaged: {engaged}\n") + except Exception: + pass + if should_start: off_ts = None if started_ts is None: @@ -382,16 +396,8 @@ def thermald_thread() -> NoReturn: # Check if we need to shut down if power_monitor.should_shutdown(peripheralState, onroad_conditions["ignition"], in_car, off_ts, started_seen): - cloudlog.info(f"shutting device down, offroad since {off_ts}") - # TODO: add function for blocking cloudlog instead of sleep - time.sleep(10) - HARDWARE.shutdown() - - # If UI has crashed, set the brightness to reasonable non-zero value - ui_running = "ui" in (p.name for p in sm["managerState"].processes if p.running) - if ui_running_prev and not ui_running: - HARDWARE.set_screen_brightness(20) - ui_running_prev = ui_running + cloudlog.warning(f"shutting device down, offroad since {off_ts}") + params.put_bool("DoShutdown", True) msg.deviceState.chargingError = current_filter.x > 0. and msg.deviceState.batteryPercent < 90 # if current is positive, then battery is being discharged msg.deviceState.started = started_ts is not None and onroad_conditions["ignition"] @@ -411,6 +417,27 @@ def thermald_thread() -> NoReturn: should_start_prev = should_start startup_conditions_prev = startup_conditions.copy() + # Log to statsd + statlog.gauge("free_space_percent", msg.deviceState.freeSpacePercent) + statlog.gauge("gpu_usage_percent", msg.deviceState.gpuUsagePercent) + statlog.gauge("memory_usage_percent", msg.deviceState.memoryUsagePercent) + for i, usage in enumerate(msg.deviceState.cpuUsagePercent): + statlog.gauge(f"cpu{i}_usage_percent", usage) + for i, temp in enumerate(msg.deviceState.cpuTempC): + statlog.gauge(f"cpu{i}_temperature", temp) + for i, temp in enumerate(msg.deviceState.gpuTempC): + statlog.gauge(f"gpu{i}_temperature", temp) + statlog.gauge("memory_temperature", msg.deviceState.memoryTempC) + statlog.gauge("ambient_temperature", msg.deviceState.ambientTempC) + for i, temp in enumerate(msg.deviceState.pmicTempC): + statlog.gauge(f"pmic{i}_temperature", temp) + for i, temp in enumerate(last_hw_state.nvme_temps): + statlog.gauge(f"nvme_temperature{i}", temp) + for i, temp in enumerate(last_hw_state.modem_temps): + statlog.gauge(f"modem_temperature{i}", temp) + statlog.gauge("fan_speed_percent_desired", msg.deviceState.fanSpeedPercentDesired) + statlog.gauge("screen_brightness_percent", msg.deviceState.screenBrightnessPercent) + # report to server once every 10 minutes if (count % int(600. / DT_TRML)) == 0: if EON and started_ts is None and msg.deviceState.memoryUsagePercent > 40: @@ -418,7 +445,7 @@ def thermald_thread() -> NoReturn: cloudlog.event("STATUS_PACKET", count=count, - pandaStates=(strip_deprecated_keys(pandaStates.to_dict()) if pandaStates else None), + pandaStates=[strip_deprecated_keys(p.to_dict()) for p in pandaStates], peripheralState=strip_deprecated_keys(peripheralState.to_dict()), location=(strip_deprecated_keys(sm["gpsLocationExternal"].to_dict()) if sm.alive["gpsLocationExternal"] else None), deviceState=strip_deprecated_keys(msg.to_dict())) @@ -426,8 +453,28 @@ def thermald_thread() -> NoReturn: count += 1 -def main() -> NoReturn: - thermald_thread() +def main(): + hw_queue = queue.Queue(maxsize=1) + end_event = threading.Event() + + threads = [ + threading.Thread(target=hw_state_thread, args=(end_event, hw_queue)), + threading.Thread(target=thermald_thread, args=(end_event, hw_queue)), + ] + + for t in threads: + t.start() + + try: + while True: + time.sleep(1) + if not all(t.is_alive() for t in threads): + break + finally: + end_event.set() + + for t in threads: + t.join() if __name__ == "__main__": diff --git a/selfdrive/tombstoned.py b/selfdrive/tombstoned.py index df96c222c0b559..459ab2f0f933d7 100755 --- a/selfdrive/tombstoned.py +++ b/selfdrive/tombstoned.py @@ -9,14 +9,12 @@ import glob from typing import NoReturn -import sentry_sdk - -from common.params import Params from common.file_helpers import mkdirs_exists_ok -from selfdrive.hardware import TICI, HARDWARE +from selfdrive.hardware import TICI from selfdrive.loggerd.config import ROOT +import selfdrive.sentry as sentry from selfdrive.swaglog import cloudlog -from selfdrive.version import get_branch, get_commit, is_dirty, get_origin, get_version +from selfdrive.version import get_commit MAX_SIZE = 100000 * 10 # mal size is 40-100k, allow up to 1M if TICI: @@ -32,16 +30,6 @@ def safe_fn(s): return "".join(c for c in s if c.isalnum() or c in extra).rstrip() -def sentry_report(fn, message, contents): - cloudlog.error({'tombstone': message}) - - with sentry_sdk.configure_scope() as scope: - scope.set_extra("tombstone_fn", fn) - scope.set_extra("tombstone", contents) - sentry_sdk.capture_message(message=message) - sentry_sdk.flush() - - def clear_apport_folder(): for f in glob.glob(APPORT_DIR + '*'): try: @@ -104,7 +92,7 @@ def report_tombstone_android(fn): if fault_idx >= 0: message = message[:fault_idx] - sentry_report(fn, message, contents) + sentry.report_tombstone(fn, message, contents) # Copy crashlog to upload folder clean_path = executable.replace('./', '').replace('/', '_') @@ -178,7 +166,7 @@ def report_tombstone_apport(fn): contents = stacktrace + "\n\n" + contents message = message + " - " + crash_function - sentry_report(fn, message, contents) + sentry.report_tombstone(fn, message, contents) # Copy crashlog to upload folder clean_path = path.replace('/', '_') @@ -199,20 +187,11 @@ def report_tombstone_apport(fn): def main() -> NoReturn: - clear_apport_folder() # Clear apport folder on start, otherwise duplicate crashes won't register - initial_tombstones = set(get_tombstones()) + sentry.init(sentry.SentryProject.SELFDRIVE_NATIVE) - sentry_sdk.utils.MAX_STRING_LENGTH = 8192 - sentry_sdk.init("https://a40f22e13cbc4261873333c125fc9d38@o33823.ingest.sentry.io/157615", - default_integrations=False, release=get_version()) - - dongle_id = Params().get("DongleId", encoding='utf-8') - sentry_sdk.set_user({"id": dongle_id}) - sentry_sdk.set_tag("dirty", is_dirty()) - sentry_sdk.set_tag("origin", get_origin()) - sentry_sdk.set_tag("branch", get_branch()) - sentry_sdk.set_tag("commit", get_commit()) - sentry_sdk.set_tag("device", HARDWARE.get_device_type()) + # Clear apport folder on start, otherwise duplicate crashes won't register + clear_apport_folder() + initial_tombstones = set(get_tombstones()) while True: now_tombstones = set(get_tombstones()) diff --git a/selfdrive/ui/SConscript b/selfdrive/ui/SConscript index 62f93c13a9992e..ed057bf54b5354 100644 --- a/selfdrive/ui/SConscript +++ b/selfdrive/ui/SConscript @@ -115,10 +115,10 @@ if GetOption('extras'): if arch in ['x86_64', 'Darwin'] or GetOption('extras'): qt_env['CXXFLAGS'] += ["-Wno-deprecated-declarations"] - replay_lib_src = ["replay/replay.cc", "replay/camera.cc", "replay/filereader.cc", "replay/logreader.cc", "replay/framereader.cc", "replay/route.cc", "replay/util.cc"] + replay_lib_src = ["replay/replay.cc", "replay/consoleui.cc", "replay/camera.cc", "replay/filereader.cc", "replay/logreader.cc", "replay/framereader.cc", "replay/route.cc", "replay/util.cc"] replay_lib = qt_env.Library("qt_replay", replay_lib_src, LIBS=base_libs) - replay_libs = [replay_lib, 'avutil', 'avcodec', 'avformat', 'bz2', 'curl', 'yuv'] + qt_libs + replay_libs = [replay_lib, 'avutil', 'avcodec', 'avformat', 'bz2', 'curl', 'yuv', 'ncurses'] + qt_libs qt_env.Program("replay/replay", ["replay/main.cc"], LIBS=replay_libs) qt_env.Program("watch3", ["watch3.cc"], LIBS=qt_libs + ['common', 'json11', 'zmq', 'visionipc', 'messaging']) diff --git a/selfdrive/ui/qt/offroad/networking.cc b/selfdrive/ui/qt/offroad/networking.cc index 2d5651dcce06fc..9c2088b1d79b83 100644 --- a/selfdrive/ui/qt/offroad/networking.cc +++ b/selfdrive/ui/qt/offroad/networking.cc @@ -99,14 +99,12 @@ void Networking::wrongPassword(const QString &ssid) { } } -void Networking::showEvent(QShowEvent* event) { - // Wait to refresh to avoid delay when showing Networking widget - QTimer::singleShot(300, this, [=]() { - if (this->isVisible()) { - wifi->refreshNetworks(); - refresh(); - } - }); +void Networking::showEvent(QShowEvent *event) { + wifi->start(); +} + +void Networking::hideEvent(QHideEvent *event) { + wifi->stop(); } // AdvancedNetworking functions diff --git a/selfdrive/ui/qt/offroad/networking.h b/selfdrive/ui/qt/offroad/networking.h index 037ef82f6798a2..e78d65ef0f1b48 100644 --- a/selfdrive/ui/qt/offroad/networking.h +++ b/selfdrive/ui/qt/offroad/networking.h @@ -65,6 +65,7 @@ class Networking : public QFrame { protected: void showEvent(QShowEvent* event) override; + void hideEvent(QHideEvent* event) override; public slots: void refresh(); diff --git a/selfdrive/ui/qt/offroad/settings.cc b/selfdrive/ui/qt/offroad/settings.cc index 83802a1d9076d5..73dbf0a845f6e2 100644 --- a/selfdrive/ui/qt/offroad/settings.cc +++ b/selfdrive/ui/qt/offroad/settings.cc @@ -53,12 +53,6 @@ TogglesPanel::TogglesPanel(SettingsWindow *parent) : ListWidget(parent) { "Display speed in km/h instead of mph.", "../assets/offroad/icon_metric.png", }, - { - "CommunityFeaturesToggle", - "Enable Community Features", - "Use features, such as community supported hardware, from the open source community that are not maintained or supported by comma.ai and have not been confirmed to meet the standard safety model. Be extra cautious when using these features", - "../assets/offroad/icon_shell.png", - }, { "RecordFront", "Record and Upload Driver Camera", @@ -164,6 +158,10 @@ DevicePanel::DevicePanel(SettingsWindow *parent) : ListWidget(parent) { power_layout->addWidget(poweroff_btn); QObject::connect(poweroff_btn, &QPushButton::clicked, this, &DevicePanel::poweroff); + if (Hardware::TICI()) { + connect(uiState(), &UIState::offroadTransition, poweroff_btn, &QPushButton::setVisible); + } + setStyleSheet(R"( #reboot_btn { height: 120px; border-radius: 15px; background-color: #393939; } #reboot_btn:pressed { background-color: #4a4a4a; } @@ -176,7 +174,7 @@ DevicePanel::DevicePanel(SettingsWindow *parent) : ListWidget(parent) { void DevicePanel::updateCalibDescription() { QString desc = "openpilot requires the device to be mounted within 4° left or right and " - "within 5° up or down. openpilot is continuously calibrating, resetting is rarely required."; + "within 5° up or 8° down. openpilot is continuously calibrating, resetting is rarely required."; std::string calib_bytes = Params().get("CalibrationParams"); if (!calib_bytes.empty()) { try { diff --git a/selfdrive/ui/qt/offroad/wifiManager.cc b/selfdrive/ui/qt/offroad/wifiManager.cc index 09701739299ca2..32ff0918efd491 100644 --- a/selfdrive/ui/qt/offroad/wifiManager.cc +++ b/selfdrive/ui/qt/offroad/wifiManager.cc @@ -1,26 +1,9 @@ #include "selfdrive/ui/qt/offroad/wifiManager.h" -#include -#include -#include - #include "selfdrive/common/params.h" #include "selfdrive/common/swaglog.h" #include "selfdrive/ui/qt/util.h" -template -T get_response(const QDBusMessage &response) { - QVariant first = response.arguments().at(0); - QDBusVariant dbvFirst = first.value(); - QVariant vFirst = dbvFirst.variant(); - if (vFirst.canConvert()) { - return vFirst.value(); - } else { - LOGE("Variant unpacking failure"); - return T(); - } -} - bool compare_by_strength(const Network &a, const Network &b) { if (a.connected == ConnectedType::CONNECTED) return true; if (b.connected == ConnectedType::CONNECTED) return false; @@ -29,10 +12,34 @@ bool compare_by_strength(const Network &a, const Network &b) { return a.strength > b.strength; } -WifiManager::WifiManager(QWidget* parent) : QWidget(parent) { +template +T call(const QString &path, const QString &interface, const QString &method, Args &&...args) { + QDBusInterface nm = QDBusInterface(NM_DBUS_SERVICE, path, interface, QDBusConnection::systemBus()); + nm.setTimeout(DBUS_TIMEOUT); + QDBusMessage response = nm.call(method, args...); + if constexpr (std::is_same_v) { + return response; + } else if (response.arguments().count() >= 1) { + QVariant vFirst = response.arguments().at(0).value().variant(); + if (vFirst.canConvert()) { + return vFirst.value(); + } + QDebug critical = qCritical(); + critical << "Variant unpacking failure :" << method << ','; + (critical << ... << args); + } + return T(); +} + +template +QDBusPendingCall asyncCall(const QString &path, const QString &interface, const QString &method, Args &&...args) { + QDBusInterface nm = QDBusInterface(NM_DBUS_SERVICE, path, interface, QDBusConnection::systemBus()); + return nm.asyncCall(method, args...); +} + +WifiManager::WifiManager(QObject *parent) : QObject(parent) { qDBusRegisterMetaType(); qDBusRegisterMetaType(); - connecting_to_network = ""; // Set tethering ssid as "weedle" + first 4 characters of a dongle id tethering_ssid = "weedle"; @@ -44,97 +51,83 @@ WifiManager::WifiManager(QWidget* parent) : QWidget(parent) { if (!adapter.isEmpty()) { setup(); } else { - bus.connect(NM_DBUS_SERVICE, NM_DBUS_PATH, NM_DBUS_INTERFACE, "DeviceAdded", this, SLOT(deviceAdded(QDBusObjectPath))); + QDBusConnection::systemBus().connect(NM_DBUS_SERVICE, NM_DBUS_PATH, NM_DBUS_INTERFACE, "DeviceAdded", this, SLOT(deviceAdded(QDBusObjectPath))); } - QTimer* timer = new QTimer(this); - QObject::connect(timer, &QTimer::timeout, this, [=]() { - if (!adapter.isEmpty() && this->isVisible()) { - requestScan(); - } - }); - timer->start(5000); + timer.callOnTimeout(this, &WifiManager::requestScan); } void WifiManager::setup() { - QDBusInterface nm(NM_DBUS_SERVICE, adapter, NM_DBUS_INTERFACE_DEVICE, bus); + auto bus = QDBusConnection::systemBus(); bus.connect(NM_DBUS_SERVICE, adapter, NM_DBUS_INTERFACE_DEVICE, "StateChanged", this, SLOT(stateChange(unsigned int, unsigned int, unsigned int))); bus.connect(NM_DBUS_SERVICE, adapter, NM_DBUS_INTERFACE_PROPERTIES, "PropertiesChanged", this, SLOT(propertyChange(QString, QVariantMap, QStringList))); bus.connect(NM_DBUS_SERVICE, NM_DBUS_PATH_SETTINGS, NM_DBUS_INTERFACE_SETTINGS, "ConnectionRemoved", this, SLOT(connectionRemoved(QDBusObjectPath))); bus.connect(NM_DBUS_SERVICE, NM_DBUS_PATH_SETTINGS, NM_DBUS_INTERFACE_SETTINGS, "NewConnection", this, SLOT(newConnection(QDBusObjectPath))); - QDBusInterface device_props(NM_DBUS_SERVICE, adapter, NM_DBUS_INTERFACE_PROPERTIES, bus); - device_props.setTimeout(DBUS_TIMEOUT); - QDBusMessage response = device_props.call("Get", NM_DBUS_INTERFACE_DEVICE, "State"); - raw_adapter_state = get_response(response); + raw_adapter_state = call(adapter, NM_DBUS_INTERFACE_PROPERTIES, "Get", NM_DBUS_INTERFACE_DEVICE, "State"); + activeAp = call(adapter, NM_DBUS_INTERFACE_PROPERTIES, "Get", NM_DBUS_INTERFACE_DEVICE_WIRELESS, "ActiveAccessPoint").path(); - initActiveAp(); initConnections(); requestScan(); } +void WifiManager::start() { + timer.start(5000); + refreshNetworks(); +} + +void WifiManager::stop() { + timer.stop(); +} + void WifiManager::refreshNetworks() { - if (adapter.isEmpty()) { - return; - } + if (adapter.isEmpty() || !timer.isActive()) return; + + QDBusPendingCall pending_call = asyncCall(adapter, NM_DBUS_INTERFACE_DEVICE_WIRELESS, "GetAllAccessPoints"); + QDBusPendingCallWatcher *watcher = new QDBusPendingCallWatcher(pending_call); + QObject::connect(watcher, &QDBusPendingCallWatcher::finished, this, &WifiManager::refreshFinished); +} + +void WifiManager::refreshFinished(QDBusPendingCallWatcher *watcher) { + ipv4_address = getIp4Address(); seenNetworks.clear(); - ipv4_address = get_ipv4_address(); - QDBusInterface nm(NM_DBUS_SERVICE, adapter, NM_DBUS_INTERFACE_DEVICE_WIRELESS, bus); - nm.setTimeout(DBUS_TIMEOUT); + const QDBusReply> wather_reply = *watcher; + for (const QDBusObjectPath &path : wather_reply.value()) { + QDBusReply replay = call(path.path(), NM_DBUS_INTERFACE_PROPERTIES, "GetAll", NM_DBUS_INTERFACE_ACCESS_POINT); + auto properties = replay.value(); - const QDBusReply> &response = nm.call("GetAllAccessPoints"); - for (const QDBusObjectPath &path : response.value()) { - const QByteArray &ssid = get_property(path.path(), "Ssid"); - unsigned int strength = get_ap_strength(path.path()); - if (ssid.isEmpty() || (seenNetworks.contains(ssid) && - strength <= seenNetworks.value(ssid).strength)) { - continue; - } - SecurityType security = getSecurityType(path.path()); - ConnectedType ctype; - QString activeSsid = (activeAp != "" && activeAp != "/") ? get_property(activeAp, "Ssid") : ""; - if (ssid != activeSsid) { - ctype = ConnectedType::DISCONNECTED; - } else { - if (ssid == connecting_to_network) { - ctype = ConnectedType::CONNECTING; - } else { - ctype = ConnectedType::CONNECTED; - } - } - Network network = {ssid, strength, ctype, security}; - seenNetworks[ssid] = network; - } -} + const QByteArray ssid = properties["Ssid"].toByteArray(); + uint32_t strength = properties["Strength"].toUInt(); + if (ssid.isEmpty() || (seenNetworks.contains(ssid) && strength <= seenNetworks[ssid].strength)) continue; -QString WifiManager::get_ipv4_address() { - if (raw_adapter_state != NM_DEVICE_STATE_ACTIVATED) { - return ""; + SecurityType security = getSecurityType(properties); + ConnectedType ctype = ConnectedType::DISCONNECTED; + if (path.path() == activeAp) { + ctype = (ssid == connecting_to_network) ? ConnectedType::CONNECTING : ConnectedType::CONNECTED; + } + seenNetworks[ssid] = {ssid, strength, ctype, security}; } - QVector conns = get_active_connections(); - for (auto &p : conns) { - QDBusInterface nm(NM_DBUS_SERVICE, p.path(), NM_DBUS_INTERFACE_PROPERTIES, bus); - nm.setTimeout(DBUS_TIMEOUT); - QDBusObjectPath pth = get_response(nm.call("Get", NM_DBUS_INTERFACE_ACTIVE_CONNECTION, "Ip4Config")); - QString ip4config = pth.path(); + emit refreshSignal(); + watcher->deleteLater(); +} - QString type = get_response(nm.call("Get", NM_DBUS_INTERFACE_ACTIVE_CONNECTION, "Type")); +QString WifiManager::getIp4Address() { + if (raw_adapter_state != NM_DEVICE_STATE_ACTIVATED) return ""; + for (const auto &p : getActiveConnections()) { + QString type = call(p.path(), NM_DBUS_INTERFACE_PROPERTIES, "Get", NM_DBUS_INTERFACE_ACTIVE_CONNECTION, "Type"); if (type == "802-11-wireless") { - QDBusInterface nm2(NM_DBUS_SERVICE, ip4config, NM_DBUS_INTERFACE_PROPERTIES, bus); - nm2.setTimeout(DBUS_TIMEOUT); - - const QDBusArgument &arr = get_response(nm2.call("Get", NM_DBUS_INTERFACE_IP4_CONFIG, "AddressData")); - QMap pth2; + auto ip4config = call(p.path(), NM_DBUS_INTERFACE_PROPERTIES, "Get", NM_DBUS_INTERFACE_ACTIVE_CONNECTION, "Ip4Config"); + const auto &arr = call(ip4config.path(), NM_DBUS_INTERFACE_PROPERTIES, "Get", NM_DBUS_INTERFACE_IP4_CONFIG, "AddressData"); + QVariantMap path; arr.beginArray(); while (!arr.atEnd()) { - arr >> pth2; - QString ipv4 = pth2.value("address").value(); + arr >> path; arr.endArray(); - return ipv4; + return path.value("address").value(); } arr.endArray(); } @@ -142,10 +135,10 @@ QString WifiManager::get_ipv4_address() { return ""; } -SecurityType WifiManager::getSecurityType(const QString &path) { - int sflag = get_property(path, "Flags").toInt(); - int wpaflag = get_property(path, "WpaFlags").toInt(); - int rsnflag = get_property(path, "RsnFlags").toInt(); +SecurityType WifiManager::getSecurityType(const QVariantMap &properties) { + int sflag = properties["Flags"].toUInt(); + int wpaflag = properties["WpaFlags"].toUInt(); + int rsnflag = properties["RsnFlags"].toUInt(); int wpa_props = wpaflag | rsnflag; // obtained by looking at flags of networks in the office as reported by an Android phone @@ -161,32 +154,20 @@ SecurityType WifiManager::getSecurityType(const QString &path) { } } -void WifiManager::connect(const Network &n) { - return connect(n, "", ""); -} - -void WifiManager::connect(const Network &n, const QString &password) { - return connect(n, "", password); -} - -void WifiManager::connect(const Network &n, const QString &username, const QString &password) { +void WifiManager::connect(const Network &n, const QString &password, const QString &username) { connecting_to_network = n.ssid; - // disconnect(); - forgetConnection(n.ssid); //Clear all connections that may already exist to the network we are connecting - connect(n.ssid, username, password, n.security_type); -} - -void WifiManager::connect(const QByteArray &ssid, const QString &username, const QString &password, SecurityType security_type) { + seenNetworks[n.ssid].connected = ConnectedType::CONNECTING; + forgetConnection(n.ssid); // Clear all connections that may already exist to the network we are connecting Connection connection; connection["connection"]["type"] = "802-11-wireless"; connection["connection"]["uuid"] = QUuid::createUuid().toString().remove('{').remove('}'); - connection["connection"]["id"] = "openpilot connection "+QString::fromStdString(ssid.toStdString()); + connection["connection"]["id"] = "openpilot connection " + QString::fromStdString(n.ssid.toStdString()); connection["connection"]["autoconnect-retries"] = 0; - connection["802-11-wireless"]["ssid"] = ssid; + connection["802-11-wireless"]["ssid"] = n.ssid; connection["802-11-wireless"]["mode"] = "infrastructure"; - if (security_type == SecurityType::WPA) { + if (n.security_type == SecurityType::WPA) { connection["802-11-wireless-security"]["key-mgmt"] = "wpa-psk"; connection["802-11-wireless-security"]["auth-alg"] = "open"; connection["802-11-wireless-security"]["psk"] = password; @@ -196,43 +177,30 @@ void WifiManager::connect(const QByteArray &ssid, const QString &username, const connection["ipv4"]["dns-priority"] = 600; connection["ipv6"]["method"] = "ignore"; - QDBusInterface nm_settings(NM_DBUS_SERVICE, NM_DBUS_PATH_SETTINGS, NM_DBUS_INTERFACE_SETTINGS, bus); - nm_settings.setTimeout(DBUS_TIMEOUT); - - nm_settings.call("AddConnection", QVariant::fromValue(connection)); + call(NM_DBUS_PATH_SETTINGS, NM_DBUS_INTERFACE_SETTINGS, "AddConnection", QVariant::fromValue(connection)); } void WifiManager::deactivateConnectionBySsid(const QString &ssid) { - for (QDBusObjectPath active_connection_raw : get_active_connections()) { - QString active_connection = active_connection_raw.path(); - QDBusInterface nm(NM_DBUS_SERVICE, active_connection, NM_DBUS_INTERFACE_PROPERTIES, bus); - nm.setTimeout(DBUS_TIMEOUT); - - QDBusObjectPath pth = get_response(nm.call("Get", NM_DBUS_INTERFACE_ACTIVE_CONNECTION, "SpecificObject")); + for (QDBusObjectPath active_connection : getActiveConnections()) { + auto pth = call(active_connection.path(), NM_DBUS_INTERFACE_PROPERTIES, "Get", NM_DBUS_INTERFACE_ACTIVE_CONNECTION, "SpecificObject"); if (pth.path() != "" && pth.path() != "/") { QString Ssid = get_property(pth.path(), "Ssid"); if (Ssid == ssid) { - deactivateConnection(active_connection_raw); + deactivateConnection(active_connection); + return; } } } } void WifiManager::deactivateConnection(const QDBusObjectPath &path) { - QDBusInterface nm2(NM_DBUS_SERVICE, NM_DBUS_PATH, NM_DBUS_INTERFACE, bus); - nm2.setTimeout(DBUS_TIMEOUT); - nm2.call("DeactivateConnection", QVariant::fromValue(path)); + asyncCall(NM_DBUS_PATH, NM_DBUS_INTERFACE, "DeactivateConnection", QVariant::fromValue(path)); } -QVector WifiManager::get_active_connections() { - QDBusInterface nm(NM_DBUS_SERVICE, NM_DBUS_PATH, NM_DBUS_INTERFACE_PROPERTIES, bus); - nm.setTimeout(DBUS_TIMEOUT); - - QDBusMessage response = nm.call("Get", NM_DBUS_INTERFACE, "ActiveConnections"); - const QDBusArgument &arr = get_response(response); +QVector WifiManager::getActiveConnections() { QVector conns; - QDBusObjectPath path; + const QDBusArgument &arr = call(NM_DBUS_PATH, NM_DBUS_INTERFACE_PROPERTIES, "Get", NM_DBUS_INTERFACE, "ActiveConnections"); arr.beginArray(); while (!arr.atEnd()) { arr >> path; @@ -249,57 +217,26 @@ bool WifiManager::isKnownConnection(const QString &ssid) { void WifiManager::forgetConnection(const QString &ssid) { const QDBusObjectPath &path = getConnectionPath(ssid); if (!path.path().isEmpty()) { - QDBusInterface nm2(NM_DBUS_SERVICE, path.path(), NM_DBUS_INTERFACE_SETTINGS_CONNECTION, bus); - nm2.call("Delete"); + call(path.path(), NM_DBUS_INTERFACE_SETTINGS_CONNECTION, "Delete"); } } uint WifiManager::getAdapterType(const QDBusObjectPath &path) { - QDBusInterface device_props(NM_DBUS_SERVICE, path.path(), NM_DBUS_INTERFACE_PROPERTIES, bus); - device_props.setTimeout(DBUS_TIMEOUT); - return get_response(device_props.call("Get", NM_DBUS_INTERFACE_DEVICE, "DeviceType")); -} - -bool WifiManager::isWirelessAdapter(const QDBusObjectPath &path) { - return getAdapterType(path) == NM_DEVICE_TYPE_WIFI; + return call(path.path(), NM_DBUS_INTERFACE_PROPERTIES, "Get", NM_DBUS_INTERFACE_DEVICE, "DeviceType"); } void WifiManager::requestScan() { - QDBusInterface nm(NM_DBUS_SERVICE, adapter, NM_DBUS_INTERFACE_DEVICE_WIRELESS, bus); - nm.setTimeout(DBUS_TIMEOUT); - nm.call("RequestScan", QVariantMap()); -} - -uint WifiManager::get_wifi_device_state() { - QDBusInterface device_props(NM_DBUS_SERVICE, adapter, NM_DBUS_INTERFACE_PROPERTIES, bus); - device_props.setTimeout(DBUS_TIMEOUT); - - QDBusMessage response = device_props.call("Get", NM_DBUS_INTERFACE_DEVICE, "State"); - uint resp = get_response(response); - return resp; + if (!adapter.isEmpty()) { + asyncCall(adapter, NM_DBUS_INTERFACE_DEVICE_WIRELESS, "RequestScan", QVariantMap()); + } } QByteArray WifiManager::get_property(const QString &network_path , const QString &property) { - QDBusInterface device_props(NM_DBUS_SERVICE, network_path, NM_DBUS_INTERFACE_PROPERTIES, bus); - device_props.setTimeout(DBUS_TIMEOUT); - - QDBusMessage response = device_props.call("Get", NM_DBUS_INTERFACE_ACCESS_POINT, property); - return get_response(response); -} - -unsigned int WifiManager::get_ap_strength(const QString &network_path) { - QDBusInterface device_props(NM_DBUS_SERVICE, network_path, NM_DBUS_INTERFACE_PROPERTIES, bus); - device_props.setTimeout(DBUS_TIMEOUT); - - QDBusMessage response = device_props.call("Get", NM_DBUS_INTERFACE_ACCESS_POINT, "Strength"); - return get_response(response); + return call(network_path, NM_DBUS_INTERFACE_PROPERTIES, "Get", NM_DBUS_INTERFACE_ACCESS_POINT, property); } QString WifiManager::getAdapter(const uint adapter_type) { - QDBusInterface nm(NM_DBUS_SERVICE, NM_DBUS_PATH, NM_DBUS_INTERFACE, bus); - nm.setTimeout(DBUS_TIMEOUT); - - const QDBusReply> &response = nm.call("GetDevices"); + QDBusReply> response = call(NM_DBUS_PATH, NM_DBUS_INTERFACE, "GetDevices"); for (const QDBusObjectPath &path : response.value()) { if (getAdapterType(path) == adapter_type) { return path.path(); @@ -315,29 +252,21 @@ void WifiManager::stateChange(unsigned int new_state, unsigned int previous_stat emit wrongPassword(connecting_to_network); } else if (new_state == NM_DEVICE_STATE_ACTIVATED) { connecting_to_network = ""; - if (this->isVisible()) { - refreshNetworks(); - emit refreshSignal(); - } + refreshNetworks(); } } // https://developer.gnome.org/NetworkManager/stable/gdbus-org.freedesktop.NetworkManager.Device.Wireless.html void WifiManager::propertyChange(const QString &interface, const QVariantMap &props, const QStringList &invalidated_props) { if (interface == NM_DBUS_INTERFACE_DEVICE_WIRELESS && props.contains("LastScan")) { - if (this->isVisible() || firstScan) { - refreshNetworks(); - emit refreshSignal(); - firstScan = false; - } + refreshNetworks(); } else if (interface == NM_DBUS_INTERFACE_DEVICE_WIRELESS && props.contains("ActiveAccessPoint")) { - const QDBusObjectPath &path = props.value("ActiveAccessPoint").value(); - activeAp = path.path(); + activeAp = props.value("ActiveAccessPoint").value().path(); } } void WifiManager::deviceAdded(const QDBusObjectPath &path) { - if (isWirelessAdapter(path) && (adapter.isEmpty() || adapter == "/")) { + if (getAdapterType(path) == NM_DEVICE_TYPE_WIFI && (adapter.isEmpty() || adapter == "/")) { adapter = path.path(); setup(); } @@ -348,7 +277,7 @@ void WifiManager::connectionRemoved(const QDBusObjectPath &path) { } void WifiManager::newConnection(const QDBusObjectPath &path) { - const Connection &settings = getConnectionSettings(path); + Connection settings = getConnectionSettings(path); if (settings.value("connection").value("type") == "802-11-wireless") { knownConnections[path] = settings.value("802-11-wireless").value("ssid").toString(); if (knownConnections[path] != tethering_ssid) { @@ -357,34 +286,18 @@ void WifiManager::newConnection(const QDBusObjectPath &path) { } } -void WifiManager::disconnect() { - if (activeAp != "" && activeAp != "/") { - deactivateConnectionBySsid(get_property(activeAp, "Ssid")); - } -} - QDBusObjectPath WifiManager::getConnectionPath(const QString &ssid) { - for (const QString &conn_ssid : knownConnections) { - if (ssid == conn_ssid) { - return knownConnections.key(conn_ssid); - } - } - return QDBusObjectPath(); + return knownConnections.key(ssid); } Connection WifiManager::getConnectionSettings(const QDBusObjectPath &path) { - QDBusInterface nm(NM_DBUS_SERVICE, path.path(), NM_DBUS_INTERFACE_SETTINGS_CONNECTION, bus); - nm.setTimeout(DBUS_TIMEOUT); - return QDBusReply(nm.call("GetSettings")).value(); + return QDBusReply(call(path.path(), NM_DBUS_INTERFACE_SETTINGS_CONNECTION, "GetSettings")).value(); } void WifiManager::initConnections() { - QDBusInterface nm(NM_DBUS_SERVICE, NM_DBUS_PATH_SETTINGS, NM_DBUS_INTERFACE_SETTINGS, bus); - nm.setTimeout(DBUS_TIMEOUT); - - const QDBusReply> response = nm.call("ListConnections"); + const QDBusReply> response = call(NM_DBUS_PATH_SETTINGS, NM_DBUS_INTERFACE_SETTINGS, "ListConnections"); for (const QDBusObjectPath &path : response.value()) { - const Connection &settings = getConnectionSettings(path); + const Connection settings = getConnectionSettings(path); if (settings.value("connection").value("type") == "802-11-wireless") { knownConnections[path] = settings.value("802-11-wireless").value("ssid").toString(); } else if (path.path() != "/") { @@ -397,40 +310,29 @@ void WifiManager::activateWifiConnection(const QString &ssid) { const QDBusObjectPath &path = getConnectionPath(ssid); if (!path.path().isEmpty()) { connecting_to_network = ssid; - QDBusInterface nm3(NM_DBUS_SERVICE, NM_DBUS_PATH, NM_DBUS_INTERFACE, bus); - nm3.setTimeout(DBUS_TIMEOUT); - nm3.call("ActivateConnection", QVariant::fromValue(path), QVariant::fromValue(QDBusObjectPath(adapter)), QVariant::fromValue(QDBusObjectPath("/"))); + asyncCall(NM_DBUS_PATH, NM_DBUS_INTERFACE, "ActivateConnection", QVariant::fromValue(path), QVariant::fromValue(QDBusObjectPath(adapter)), QVariant::fromValue(QDBusObjectPath("/"))); } } void WifiManager::activateModemConnection(const QDBusObjectPath &path) { QString modem = getAdapter(NM_DEVICE_TYPE_MODEM); if (!path.path().isEmpty() && !modem.isEmpty()) { - QDBusInterface nm3(NM_DBUS_SERVICE, NM_DBUS_PATH, NM_DBUS_INTERFACE, bus); - nm3.setTimeout(DBUS_TIMEOUT); - nm3.call("ActivateConnection", QVariant::fromValue(path), QVariant::fromValue(QDBusObjectPath(modem)), QVariant::fromValue(QDBusObjectPath("/"))); + asyncCall(NM_DBUS_PATH, NM_DBUS_INTERFACE, "ActivateConnection", QVariant::fromValue(path), QVariant::fromValue(QDBusObjectPath(modem)), QVariant::fromValue(QDBusObjectPath("/"))); } } // function matches tici/hardware.py NetworkType WifiManager::currentNetworkType() { - QDBusInterface nm(NM_DBUS_SERVICE, NM_DBUS_PATH, NM_DBUS_INTERFACE_PROPERTIES, bus); - nm.setTimeout(DBUS_TIMEOUT); - const QDBusObjectPath &primary_conn = get_response(nm.call("Get", NM_DBUS_INTERFACE, "PrimaryConnection")); - - QDBusInterface nm2(NM_DBUS_SERVICE, primary_conn.path(), NM_DBUS_INTERFACE_PROPERTIES, bus); - nm.setTimeout(DBUS_TIMEOUT); - const QString &primary_type = get_response(nm2.call("Get", NM_DBUS_INTERFACE_ACTIVE_CONNECTION, "Type")); + auto primary_conn = call(NM_DBUS_PATH, NM_DBUS_INTERFACE_PROPERTIES, "Get", NM_DBUS_INTERFACE, "PrimaryConnection"); + auto primary_type = call(primary_conn.path(), NM_DBUS_INTERFACE_PROPERTIES, "Get", NM_DBUS_INTERFACE_ACTIVE_CONNECTION, "Type"); if (primary_type == "802-3-ethernet") { return NetworkType::ETHERNET; } else if (primary_type == "802-11-wireless" && !isTetheringEnabled()) { return NetworkType::WIFI; } else { - for (const QDBusObjectPath &conn : get_active_connections()) { - QDBusInterface nm3(NM_DBUS_SERVICE, conn.path(), NM_DBUS_INTERFACE_PROPERTIES, bus); - nm3.setTimeout(DBUS_TIMEOUT); - const QString &type = get_response(nm3.call("Get", NM_DBUS_INTERFACE_ACTIVE_CONNECTION, "Type")); + for (const QDBusObjectPath &conn : getActiveConnections()) { + auto type = call(conn.path(), NM_DBUS_INTERFACE_PROPERTIES, "Get", NM_DBUS_INTERFACE_ACTIVE_CONNECTION, "Type"); if (type == "gsm") { return NetworkType::CELL; } @@ -441,13 +343,9 @@ NetworkType WifiManager::currentNetworkType() { void WifiManager::updateGsmSettings(bool roaming, QString apn) { if (!lteConnectionPath.path().isEmpty()) { - QDBusInterface nm(NM_DBUS_SERVICE, lteConnectionPath.path(), NM_DBUS_INTERFACE_SETTINGS_CONNECTION, bus); - nm.setTimeout(DBUS_TIMEOUT); - bool changes = false; bool auto_config = apn.isEmpty(); - Connection settings = QDBusReply(nm.call("GetSettings")).value(); - + Connection settings = getConnectionSettings(lteConnectionPath); if (settings.value("gsm").value("auto-config").toBool() != auto_config) { qWarning() << "Changing gsm.auto-config to" << auto_config; settings["gsm"]["auto-config"] = auto_config; @@ -467,7 +365,7 @@ void WifiManager::updateGsmSettings(bool roaming, QString apn) { } if (changes) { - nm.call("UpdateUnsaved", QVariant::fromValue(settings)); // update is temporary + call(lteConnectionPath.path(), NM_DBUS_INTERFACE_SETTINGS_CONNECTION, "UpdateUnsaved", QVariant::fromValue(settings)); // update is temporary deactivateConnection(lteConnectionPath); activateModemConnection(lteConnectionPath); } @@ -494,7 +392,7 @@ void WifiManager::addTetheringConnection() { connection["802-11-wireless-security"]["psk"] = defaultTetheringPassword; connection["ipv4"]["method"] = "shared"; - QMap address; + QVariantMap address; address["address"] = "192.168.43.1"; address["prefix"] = 24u; connection["ipv4"]["address-data"] = QVariant::fromValue(IpConfig() << address); @@ -502,9 +400,7 @@ void WifiManager::addTetheringConnection() { connection["ipv4"]["route-metric"] = 1100; connection["ipv6"]["method"] = "ignore"; - QDBusInterface nm_settings(NM_DBUS_SERVICE, NM_DBUS_PATH_SETTINGS, NM_DBUS_INTERFACE_SETTINGS, bus); - nm_settings.setTimeout(DBUS_TIMEOUT); - nm_settings.call("AddConnection", QVariant::fromValue(connection)); + call(NM_DBUS_PATH_SETTINGS, NM_DBUS_INTERFACE_SETTINGS, "AddConnection", QVariant::fromValue(connection)); } void WifiManager::setTetheringEnabled(bool enabled) { @@ -518,15 +414,6 @@ void WifiManager::setTetheringEnabled(bool enabled) { } } -void WifiManager::initActiveAp() { - QDBusInterface device_props(NM_DBUS_SERVICE, adapter, NM_DBUS_INTERFACE_PROPERTIES, bus); - device_props.setTimeout(DBUS_TIMEOUT); - - const QDBusMessage &response = device_props.call("Get", NM_DBUS_INTERFACE_DEVICE_WIRELESS, "ActiveAccessPoint"); - activeAp = get_response(response).path(); -} - - bool WifiManager::isTetheringEnabled() { if (activeAp != "" && activeAp != "/") { return get_property(activeAp, "Ssid") == tethering_ssid; @@ -540,10 +427,7 @@ QString WifiManager::getTetheringPassword() { } const QDBusObjectPath &path = getConnectionPath(tethering_ssid); if (!path.path().isEmpty()) { - QDBusInterface nm(NM_DBUS_INTERFACE, path.path(), NM_DBUS_INTERFACE_SETTINGS_CONNECTION, bus); - nm.setTimeout(DBUS_TIMEOUT); - - const QDBusReply>> response = nm.call("GetSecrets", "802-11-wireless-security"); + QDBusReply> response = call(path.path(), NM_DBUS_INTERFACE_SETTINGS_CONNECTION, "GetSecrets", "802-11-wireless-security"); return response.value().value("802-11-wireless-security").value("psk").toString(); } return ""; @@ -552,13 +436,9 @@ QString WifiManager::getTetheringPassword() { void WifiManager::changeTetheringPassword(const QString &newPassword) { const QDBusObjectPath &path = getConnectionPath(tethering_ssid); if (!path.path().isEmpty()) { - QDBusInterface nm(NM_DBUS_INTERFACE, path.path(), NM_DBUS_INTERFACE_SETTINGS_CONNECTION, bus); - nm.setTimeout(DBUS_TIMEOUT); - - Connection settings = QDBusReply(nm.call("GetSettings")).value(); + Connection settings = getConnectionSettings(path); settings["802-11-wireless-security"]["psk"] = newPassword; - nm.call("Update", QVariant::fromValue(settings)); - + call(path.path(), NM_DBUS_INTERFACE_SETTINGS_CONNECTION, "Update", QVariant::fromValue(settings)); if (isTetheringEnabled()) { activateWifiConnection(tethering_ssid); } diff --git a/selfdrive/ui/qt/offroad/wifiManager.h b/selfdrive/ui/qt/offroad/wifiManager.h index 04defd32dd584c..49c107f555c182 100644 --- a/selfdrive/ui/qt/offroad/wifiManager.h +++ b/selfdrive/ui/qt/offroad/wifiManager.h @@ -1,7 +1,7 @@ #pragma once #include -#include +#include #include "selfdrive/ui/qt/offroad/networkmanager.h" @@ -22,8 +22,8 @@ enum class NetworkType { ETHERNET }; -typedef QMap> Connection; -typedef QVector> IpConfig; +typedef QMap Connection; +typedef QVector IpConfig; struct Network { QByteArray ssid; @@ -33,65 +33,58 @@ struct Network { }; bool compare_by_strength(const Network &a, const Network &b); -class WifiManager : public QWidget { +class WifiManager : public QObject { Q_OBJECT public: - explicit WifiManager(QWidget* parent); - - void requestScan(); QMap seenNetworks; QMap knownConnections; - QDBusObjectPath lteConnectionPath; QString ipv4_address; - void refreshNetworks(); + explicit WifiManager(QObject* parent); + void start(); + void stop(); + void requestScan(); void forgetConnection(const QString &ssid); bool isKnownConnection(const QString &ssid); void activateWifiConnection(const QString &ssid); - void activateModemConnection(const QDBusObjectPath &path); NetworkType currentNetworkType(); void updateGsmSettings(bool roaming, QString apn); - - void connect(const Network &ssid); - void connect(const Network &ssid, const QString &password); - void connect(const Network &ssid, const QString &username, const QString &password); - void disconnect(); + void connect(const Network &ssid, const QString &password = {}, const QString &username = {}); // Tethering functions void setTetheringEnabled(bool enabled); bool isTetheringEnabled(); - void addTetheringConnection(); void changeTetheringPassword(const QString &newPassword); QString getTetheringPassword(); private: QString adapter; // Path to network manager wifi-device - QDBusConnection bus = QDBusConnection::systemBus(); + QTimer timer; unsigned int raw_adapter_state; // Connection status https://developer.gnome.org/NetworkManager/1.26/nm-dbus-types.html#NMDeviceState QString connecting_to_network; QString tethering_ssid; const QString defaultTetheringPassword = "swagswagcomma"; + QString activeAp; + QDBusObjectPath lteConnectionPath; - bool firstScan = true; QString getAdapter(const uint = NM_DEVICE_TYPE_WIFI); uint getAdapterType(const QDBusObjectPath &path); bool isWirelessAdapter(const QDBusObjectPath &path); - QString get_ipv4_address(); + QString getIp4Address(); void connect(const QByteArray &ssid, const QString &username, const QString &password, SecurityType security_type); - QString activeAp; - void initActiveAp(); void deactivateConnectionBySsid(const QString &ssid); void deactivateConnection(const QDBusObjectPath &path); - QVector get_active_connections(); - uint get_wifi_device_state(); + QVector getActiveConnections(); QByteArray get_property(const QString &network_path, const QString &property); - unsigned int get_ap_strength(const QString &network_path); - SecurityType getSecurityType(const QString &path); + SecurityType getSecurityType(const QVariantMap &properties); QDBusObjectPath getConnectionPath(const QString &ssid); Connection getConnectionSettings(const QDBusObjectPath &path); void initConnections(); void setup(); + void refreshNetworks(); + void activateModemConnection(const QDBusObjectPath &path); + void addTetheringConnection(); signals: void wrongPassword(const QString &ssid); @@ -103,4 +96,5 @@ private slots: void deviceAdded(const QDBusObjectPath &path); void connectionRemoved(const QDBusObjectPath &path); void newConnection(const QDBusObjectPath &path); + void refreshFinished(QDBusPendingCallWatcher *call); }; diff --git a/selfdrive/ui/qt/onroad.cc b/selfdrive/ui/qt/onroad.cc index e35f1b2779c627..0a04484ee64bad 100644 --- a/selfdrive/ui/qt/onroad.cc +++ b/selfdrive/ui/qt/onroad.cc @@ -464,7 +464,7 @@ void NvgWindow::drawLead(QPainter &painter, const cereal::ModelDataV2::LeadDataV float g_xo = sz / 5; float g_yo = sz / 10; - QPointF glow[] = {{x + (sz * 1.35) + g_xo, y + sz + g_yo}, {x, y - g_xo}, {x - (sz * 1.35) - g_xo, y + sz + g_yo}}; + QPointF glow[] = {{x + (sz * 1.35) + g_xo, y + sz + g_yo}, {x, y - g_yo}, {x - (sz * 1.35) - g_xo, y + sz + g_yo}}; painter.setBrush(QColor(218, 202, 37, 255)); painter.drawPolygon(glow, std::size(glow)); diff --git a/selfdrive/ui/qt/setup/reset.cc b/selfdrive/ui/qt/setup/reset.cc index aa000f93593107..9ffcf7f6cf583b 100644 --- a/selfdrive/ui/qt/setup/reset.cc +++ b/selfdrive/ui/qt/setup/reset.cc @@ -9,15 +9,12 @@ #include "selfdrive/ui/qt/setup/reset.h" #define NVME "/dev/nvme0n1" -#define SDCARD "/dev/mmcblk0" #define USERDATA "/dev/disk/by-partlabel/userdata" void Reset::doReset() { // best effort to wipe nvme and sd card std::system("sudo umount " NVME); std::system("yes | sudo mkfs.ext4 " NVME); - std::system("sudo umount " SDCARD); - std::system("yes | sudo mkfs.ext4 " SDCARD); // we handle two cases here // * user-prompted factory reset diff --git a/selfdrive/ui/qt/setup/setup.cc b/selfdrive/ui/qt/setup/setup.cc index bd494327ccc260..17304665c7fe6f 100644 --- a/selfdrive/ui/qt/setup/setup.cc +++ b/selfdrive/ui/qt/setup/setup.cc @@ -10,13 +10,14 @@ #include +#include "selfdrive/common/util.h" #include "selfdrive/hardware/hw.h" #include "selfdrive/ui/qt/api.h" #include "selfdrive/ui/qt/qt_window.h" #include "selfdrive/ui/qt/offroad/networking.h" #include "selfdrive/ui/qt/widgets/input.h" -const char* USER_AGENT = "AGNOSSetup-0.1"; +const std::string USER_AGENT = "AGNOSSetup-"; const QString DASHCAM_URL = "https://dashcam.comma.ai"; void Setup::download(QString url) { @@ -26,6 +27,8 @@ void Setup::download(QString url) { return; } + auto version = util::read_file("/VERSION"); + char tmpfile[] = "/tmp/installer_XXXXXX"; FILE *fp = fdopen(mkstemp(tmpfile), "w"); @@ -34,18 +37,21 @@ void Setup::download(QString url) { curl_easy_setopt(curl, CURLOPT_WRITEDATA, fp); curl_easy_setopt(curl, CURLOPT_VERBOSE, 1L); curl_easy_setopt(curl, CURLOPT_FOLLOWLOCATION, 1L); - curl_easy_setopt(curl, CURLOPT_USERAGENT, USER_AGENT); + curl_easy_setopt(curl, CURLOPT_USERAGENT, (USER_AGENT + version).c_str()); int ret = curl_easy_perform(curl); - if (ret != CURLE_OK) { + + long res_status = 0; + curl_easy_getinfo(curl, CURLINFO_RESPONSE_CODE, &res_status); + if (ret == CURLE_OK && res_status == 200) { + rename(tmpfile, "/tmp/installer"); + emit finished(true); + } else { emit finished(false); - return; } + curl_easy_cleanup(curl); fclose(fp); - - rename(tmpfile, "/tmp/installer"); - emit finished(true); } QWidget * Setup::low_voltage() { diff --git a/selfdrive/ui/qt/window.cc b/selfdrive/ui/qt/window.cc index 0fee52f7368490..76d5a39d44034d 100644 --- a/selfdrive/ui/qt/window.cc +++ b/selfdrive/ui/qt/window.cc @@ -80,19 +80,20 @@ void MainWindow::closeSettings() { } bool MainWindow::eventFilter(QObject *obj, QEvent *event) { - if (event->type() == QEvent::MouseButtonPress || event->type() == QEvent::TouchBegin) { - device.resetInteractiveTimout(); - } + const static QSet evts({QEvent::MouseButtonPress, QEvent::MouseMove, + QEvent::TouchBegin, QEvent::TouchUpdate, QEvent::TouchEnd}); + if (evts.contains(event->type())) { + device.resetInteractiveTimout(); #ifdef QCOM - // filter out touches while in android activity - const static QSet filter_events({QEvent::MouseButtonPress, QEvent::MouseMove, QEvent::TouchBegin, QEvent::TouchUpdate, QEvent::TouchEnd}); - if (HardwareEon::launched_activity && filter_events.contains(event->type())) { - HardwareEon::check_activity(); + // filter out touches while in android activity if (HardwareEon::launched_activity) { - return true; + HardwareEon::check_activity(); + if (HardwareEon::launched_activity) { + return true; + } } - } #endif + } return false; } diff --git a/selfdrive/ui/replay/camera.cc b/selfdrive/ui/replay/camera.cc index 1912c9b5811a3b..9bcc00583eefce 100644 --- a/selfdrive/ui/replay/camera.cc +++ b/selfdrive/ui/replay/camera.cc @@ -1,7 +1,7 @@ #include "selfdrive/ui/replay/camera.h" +#include "selfdrive/ui/replay/util.h" #include -#include CameraServer::CameraServer(std::pair camera_size[MAX_CAMERAS], bool send_yuv) : send_yuv(send_yuv) { for (int i = 0; i < MAX_CAMERAS; ++i) { @@ -24,7 +24,7 @@ void CameraServer::startVipcServer() { vipc_server_.reset(new VisionIpcServer("camerad")); for (auto &cam : cameras_) { if (cam.width > 0 && cam.height > 0) { - std::cout << "camera[" << cam.type << "] frame size " << cam.width << "x" << cam.height << std::endl; + rInfo("camera[%d] frame size %dx%d", cam.type, cam.width, cam.height); vipc_server_->create_buffers(cam.rgb_type, UI_BUF_COUNT, true, cam.width, cam.height); if (send_yuv) { vipc_server_->create_buffers(cam.yuv_type, YUV_BUFFER_COUNT, false, cam.width, cam.height); @@ -61,7 +61,7 @@ void CameraServer::cameraThread(Camera &cam) { if (rgb) vipc_server_->send(rgb, &extra, false); if (yuv) vipc_server_->send(yuv, &extra, false); } else { - std::cout << "camera[" << cam.type << "] failed to get frame:" << eidx.getSegmentId() << std::endl; + rError("camera[%d] failed to get frame:", cam.type, eidx.getSegmentId()); } cam.cached_id = id + 1; diff --git a/selfdrive/ui/replay/consoleui.cc b/selfdrive/ui/replay/consoleui.cc new file mode 100644 index 00000000000000..72e2fcf80200ca --- /dev/null +++ b/selfdrive/ui/replay/consoleui.cc @@ -0,0 +1,353 @@ +#include "selfdrive/ui/replay/consoleui.h" + +#include +#include + +#include "selfdrive/common/version.h" + +namespace { + +const int BORDER_SIZE = 3; + +const std::initializer_list> keyboard_shortcuts[] = { + { + {"s", "+10s"}, + {"shift+s", "-10s"}, + {"m", "+60s"}, + {"shift+m", "-60s"}, + {"space", "Pause/Resume"}, + {"e", "Next Engagement"}, + {"d", "Next Disengagement"}, + }, + { + {"enter", "Enter seek request"}, + {"x", "+/-Replay speed"}, + {"q", "Exit"}, + }, +}; + +enum Color { + Default, + Debug, + Yellow, + Green, + Red, + BrightWhite, + Engaged, + Disengaged, +}; + +void add_str(WINDOW *w, const char *str, Color color = Color::Default, bool bold = false) { + if (color != Color::Default) wattron(w, COLOR_PAIR(color)); + if (bold) wattron(w, A_BOLD); + waddstr(w, str); + if (bold) wattroff(w, A_BOLD); + if (color != Color::Default) wattroff(w, COLOR_PAIR(color)); +} + +std::string format_seconds(int s) { + int total_minutes = s / 60; + int seconds = s % 60; + int hours = total_minutes / 60; + int minutes = total_minutes % 60; + return util::string_format("%02d:%02d:%02d", hours, minutes, seconds); +} + +} // namespace + +ConsoleUI::ConsoleUI(Replay *replay, QObject *parent) : replay(replay), sm({"carState", "liveParameters"}), QObject(parent) { + // Initialize curses + initscr(); + clear(); + curs_set(false); + cbreak(); // Line buffering disabled. pass on everything + noecho(); + keypad(stdscr, true); + nodelay(stdscr, true); // non-blocking getchar() + + // Initialize all the colors. https://www.ditig.com/256-colors-cheat-sheet + start_color(); + init_pair(Color::Debug, 246, COLOR_BLACK); // #949494 + init_pair(Color::Yellow, 184, COLOR_BLACK); + init_pair(Color::Red, COLOR_RED, COLOR_BLACK); + init_pair(Color::BrightWhite, 15, COLOR_BLACK); + init_pair(Color::Disengaged, COLOR_BLUE, COLOR_BLUE); + init_pair(Color::Engaged, 28, 28); + init_pair(Color::Green, 34, COLOR_BLACK); + + initWindows(); + + qRegisterMetaType("uint64_t"); + qRegisterMetaType("ReplyMsgType"); + installMessageHandler([this](ReplyMsgType type, const std::string msg) { + emit logMessageSignal(type, QString::fromStdString(msg)); + }); + installDownloadProgressHandler([this](uint64_t cur, uint64_t total, bool success) { + emit updateProgressBarSignal(cur, total, success); + }); + + QObject::connect(replay, &Replay::streamStarted, this, &ConsoleUI::updateSummary); + QObject::connect(¬ifier, SIGNAL(activated(int)), SLOT(readyRead())); + QObject::connect(this, &ConsoleUI::updateProgressBarSignal, this, &ConsoleUI::updateProgressBar); + QObject::connect(this, &ConsoleUI::logMessageSignal, this, &ConsoleUI::logMessage); + + sm_timer.callOnTimeout(this, &ConsoleUI::updateStatus); + sm_timer.start(100); + getch_timer.start(1000, this); + readyRead(); +} + +ConsoleUI::~ConsoleUI() { + endwin(); +} + +void ConsoleUI::initWindows() { + getmaxyx(stdscr, max_height, max_width); + w.fill(nullptr); + w[Win::Title] = newwin(1, max_width, 0, 0); + w[Win::Stats] = newwin(2, max_width - 2 * BORDER_SIZE, 2, BORDER_SIZE); + w[Win::Timeline] = newwin(4, max_width - 2 * BORDER_SIZE, 5, BORDER_SIZE); + w[Win::TimelineDesc] = newwin(1, 100, 10, BORDER_SIZE); + w[Win::CarState] = newwin(3, 100, 12, BORDER_SIZE); + w[Win::DownloadBar] = newwin(1, 100, 16, BORDER_SIZE); + if (int log_height = max_height - 27; log_height > 4) { + w[Win::LogBorder] = newwin(log_height, max_width - 2 * (BORDER_SIZE - 1), 17, BORDER_SIZE - 1); + box(w[Win::LogBorder], 0, 0); + w[Win::Log] = newwin(log_height - 2, max_width - 2 * BORDER_SIZE, 18, BORDER_SIZE); + scrollok(w[Win::Log], true); + } + w[Win::Help] = newwin(5, max_width - (2 * BORDER_SIZE), max_height - 6, BORDER_SIZE); + + // set the title bar + wbkgd(w[Win::Title], A_REVERSE); + mvwprintw(w[Win::Title], 0, 3, "openpilot replay %s", COMMA_VERSION); + + // show windows on the real screen + refresh(); + displayTimelineDesc(); + displayHelp(); + updateSummary(); + updateTimeline(); + for (auto win : w) { + if (win) wrefresh(win); + } +} + +void ConsoleUI::timerEvent(QTimerEvent *ev) { + if (ev->timerId() != getch_timer.timerId()) return; + + if (is_term_resized(max_height, max_width)) { + for (auto win : w) { + if (win) delwin(win); + } + endwin(); + clear(); + refresh(); + initWindows(); + rWarning("resize term %dx%d", max_height, max_width); + } + updateTimeline(); +} + +void ConsoleUI::updateStatus() { + auto write_item = [this](int y, int x, const char *key, const std::string &value, const char *unit, + bool bold = false, Color color = Color::BrightWhite) { + auto win = w[Win::CarState]; + wmove(win, y, x); + add_str(win, key); + add_str(win, value.c_str(), color, bold); + add_str(win, unit); + }; + static const std::pair status_text[] = { + {"loading...", Color::Red}, + {"playing", Color::Green}, + {"paused...", Color::Yellow}, + }; + + sm.update(0); + + if (status != Status::Paused) { + status = (sm.updated("carState") || sm.updated("liveParameters")) ? Status::Playing : Status::Waiting; + } + auto [status_str, status_color] = status_text[status]; + write_item(0, 0, "STATUS: ", status_str, " ", false, status_color); + std::string suffix = util::string_format(" / %s [%d/%d] ", format_seconds(replay->totalSeconds()).c_str(), + replay->currentSeconds() / 60, replay->route()->segments().size()); + write_item(0, 25, "TIME: ", format_seconds(replay->currentSeconds()), suffix.c_str(), true); + + auto p = sm["liveParameters"].getLiveParameters(); + write_item(1, 0, "STIFFNESS: ", util::string_format("%.2f %%", p.getStiffnessFactor() * 100), " "); + write_item(1, 25, "SPEED: ", util::string_format("%.2f", sm["carState"].getCarState().getVEgo()), " m/s"); + write_item(2, 0, "STEER RATIO: ", util::string_format("%.2f", p.getSteerRatio()), ""); + auto angle_offsets = util::string_format("%.2f|%.2f", p.getAngleOffsetAverageDeg(), p.getAngleOffsetDeg()); + write_item(2, 25, "ANGLE OFFSET(AVG|INSTANT): ", angle_offsets, " deg"); + + wrefresh(w[Win::CarState]); +} + +void ConsoleUI::displayHelp() { + for (int i = 0; i < std::size(keyboard_shortcuts); ++i) { + wmove(w[Win::Help], i * 2, 0); + for (auto &[key, desc] : keyboard_shortcuts[i]) { + wattron(w[Win::Help], A_REVERSE); + waddstr(w[Win::Help], (' ' + key + ' ').c_str()); + wattroff(w[Win::Help], A_REVERSE); + waddstr(w[Win::Help], (' ' + desc + ' ').c_str()); + } + } + wrefresh(w[Win::Help]); +} + +void ConsoleUI::displayTimelineDesc() { + std::tuple indicators[]{ + {Color::Engaged, " Engaged ", false}, + {Color::Disengaged, " Disengaged ", false}, + {Color::Green, " Info ", true}, + {Color::Yellow, " Warning ", true}, + {Color::Red, " Critical ", true}, + }; + for (auto [color, name, bold] : indicators) { + add_str(w[Win::TimelineDesc], "__", color, bold); + add_str(w[Win::TimelineDesc], name); + } +} + +void ConsoleUI::logMessage(ReplyMsgType type, const QString &msg) { + if (auto win = w[Win::Log]) { + Color color = Color::Default; + if (type == ReplyMsgType::Debug) { + color = Color::Debug; + } else if (type == ReplyMsgType::Warning) { + color = Color::Yellow; + } else if (type == ReplyMsgType::Critical) { + color = Color::Red; + } + add_str(win, qPrintable(msg + "\n"), color); + wrefresh(win); + } +} + +void ConsoleUI::updateProgressBar(uint64_t cur, uint64_t total, bool success) { + werase(w[Win::DownloadBar]); + if (success && cur < total) { + const int width = 35; + const float progress = cur / (double)total; + const int pos = width * progress; + wprintw(w[Win::DownloadBar], "Downloading [%s>%s] %d%% %s", std::string(pos, '=').c_str(), + std::string(width - pos, ' ').c_str(), int(progress * 100.0), formattedDataSize(total).c_str()); + } + wrefresh(w[Win::DownloadBar]); +} + +void ConsoleUI::updateSummary() { + const auto &route = replay->route(); + mvwprintw(w[Win::Stats], 0, 0, "Route: %s, %d segments", qPrintable(route->name()), route->segments().size()); + mvwprintw(w[Win::Stats], 1, 0, "Car Fingerprint: %s", replay->carFingerprint().c_str()); + wrefresh(w[Win::Stats]); +} + +void ConsoleUI::updateTimeline() { + auto win = w[Win::Timeline]; + int width = getmaxx(win); + werase(win); + + wattron(win, COLOR_PAIR(Color::Disengaged)); + mvwhline(win, 1, 0, ' ', width); + mvwhline(win, 2, 0, ' ', width); + wattroff(win, COLOR_PAIR(Color::Disengaged)); + + const int total_sec = replay->totalSeconds(); + for (auto [begin, end, type] : replay->getTimeline()) { + int start_pos = ((double)begin / total_sec) * width; + int end_pos = ((double)end / total_sec) * width; + if (type == TimelineType::Engaged) { + mvwchgat(win, 1, start_pos, end_pos - start_pos + 1, A_COLOR, Color::Engaged, NULL); + mvwchgat(win, 2, start_pos, end_pos - start_pos + 1, A_COLOR, Color::Engaged, NULL); + } else { + auto color_id = Color::Green; + if (type != TimelineType::AlertInfo) { + color_id = type == TimelineType::AlertWarning ? Color::Yellow : Color::Red; + } + mvwchgat(win, 3, start_pos, end_pos - start_pos + 1, ACS_S3, color_id, NULL); + } + } + + int cur_pos = ((double)replay->currentSeconds() / total_sec) * width; + wattron(win, COLOR_PAIR(Color::BrightWhite)); + mvwaddch(win, 0, cur_pos, ACS_VLINE); + mvwaddch(win, 3, cur_pos, ACS_VLINE); + wattroff(win, COLOR_PAIR(Color::BrightWhite)); + wrefresh(win); +} + +void ConsoleUI::readyRead() { + int c; + while ((c = getch()) != ERR) { + handleKey(c); + } +} + +void ConsoleUI::pauseReplay(bool pause) { + replay->pause(pause); + status = pause ? Status::Paused : Status::Waiting; +} + +void ConsoleUI::handleKey(char c) { + if (c == '\n') { + // pause the replay and blocking getchar() + pauseReplay(true); + updateStatus(); + getch_timer.stop(); + curs_set(true); + nodelay(stdscr, false); + + // Wait for user input + rWarning("Waiting for input..."); + int y = getmaxy(stdscr) - 9; + move(y, BORDER_SIZE); + add_str(stdscr, "Enter seek request: ", Color::BrightWhite, true); + refresh(); + + // Seek to choice + echo(); + int choice = 0; + scanw((char *)"%d", &choice); + noecho(); + pauseReplay(false); + replay->seekTo(choice, false); + + // Clean up and turn off the blocking mode + move(y, 0); + clrtoeol(); + nodelay(stdscr, true); + curs_set(false); + refresh(); + getch_timer.start(1000, this); + + } else if (c == 'x') { + if (replay->hasFlag(REPLAY_FLAG_FULL_SPEED)) { + replay->removeFlag(REPLAY_FLAG_FULL_SPEED); + rWarning("replay at normal speed"); + } else { + replay->addFlag(REPLAY_FLAG_FULL_SPEED); + rWarning("replay at full speed"); + } + } else if (c == 'e') { + replay->seekToFlag(FindFlag::nextEngagement); + } else if (c == 'd') { + replay->seekToFlag(FindFlag::nextDisEngagement); + } else if (c == 'm') { + replay->seekTo(+60, true); + } else if (c == 'M') { + replay->seekTo(-60, true); + } else if (c == 's') { + replay->seekTo(+10, true); + } else if (c == 'S') { + replay->seekTo(-10, true); + } else if (c == ' ') { + pauseReplay(!replay->isPaused()); + } else if (c == 'q' || c == 'Q') { + replay->stop(); + qApp->exit(); + } +} diff --git a/selfdrive/ui/replay/consoleui.h b/selfdrive/ui/replay/consoleui.h new file mode 100644 index 00000000000000..bce1146d4615b9 --- /dev/null +++ b/selfdrive/ui/replay/consoleui.h @@ -0,0 +1,50 @@ +#pragma once + +#include +#include +#include +#include +#include +#include + +#include "selfdrive/ui/replay/replay.h" +#include + +class ConsoleUI : public QObject { + Q_OBJECT + +public: + ConsoleUI(Replay *replay, QObject *parent = 0); + ~ConsoleUI(); + +private: + void initWindows(); + void handleKey(char c); + void displayHelp(); + void displayTimelineDesc(); + void updateTimeline(); + void updateSummary(); + void updateStatus(); + void pauseReplay(bool pause); + + enum Status { Waiting, Playing, Paused }; + enum Win { Title, Stats, Log, LogBorder, DownloadBar, Timeline, TimelineDesc, Help, CarState, Max}; + std::array w{}; + SubMaster sm; + Replay *replay; + QBasicTimer getch_timer; + QTimer sm_timer; + QSocketNotifier notifier{0, QSocketNotifier::Read, this}; + int max_width, max_height; + Status status = Status::Waiting; + +signals: + void updateProgressBarSignal(uint64_t cur, uint64_t total, bool success); + void logMessageSignal(ReplyMsgType type, const QString &msg); + +private slots: + void readyRead(); + void timerEvent(QTimerEvent *ev); + void updateProgressBar(uint64_t cur, uint64_t total, bool success); + void logMessage(ReplyMsgType type, const QString &msg); +}; diff --git a/selfdrive/ui/replay/filereader.cc b/selfdrive/ui/replay/filereader.cc index 1585bb42d207fc..b57a62d501a84c 100644 --- a/selfdrive/ui/replay/filereader.cc +++ b/selfdrive/ui/replay/filereader.cc @@ -1,7 +1,6 @@ #include "selfdrive/ui/replay/filereader.h" #include -#include #include "selfdrive/common/util.h" #include "selfdrive/ui/replay/util.h" @@ -35,13 +34,12 @@ std::string FileReader::read(const std::string &file, std::atomic *abort) std::string FileReader::download(const std::string &url, std::atomic *abort) { for (int i = 0; i <= max_retries_ && !(abort && *abort); ++i) { + if (i > 0) rWarning("download failed, retrying %d", i); + std::string result = httpGet(url, chunk_size_, abort); if (!result.empty()) { return result; } - if (i != max_retries_ && !(abort && *abort)) { - std::cout << "download failed, retrying " << i + 1 << std::endl; - } } return {}; } diff --git a/selfdrive/ui/replay/framereader.cc b/selfdrive/ui/replay/framereader.cc index 59e43cb58f89ff..ef90c283555ae0 100644 --- a/selfdrive/ui/replay/framereader.cc +++ b/selfdrive/ui/replay/framereader.cc @@ -1,4 +1,5 @@ #include "selfdrive/ui/replay/framereader.h" +#include "selfdrive/ui/replay/util.h" #include #include "libyuv.h" @@ -29,7 +30,7 @@ enum AVPixelFormat get_hw_format(AVCodecContext *ctx, const enum AVPixelFormat * for (const enum AVPixelFormat *p = pix_fmts; *p != -1; p++) { if (*p == *hw_pix_fmt) return *p; } - printf("Please run replay with the --no-cuda flag!\n"); + rWarning("Please run replay with the --no-cuda flag!"); // fallback to YUV420p *hw_pix_fmt = AV_PIX_FMT_NONE; return AV_PIX_FMT_YUV420P; @@ -37,7 +38,9 @@ enum AVPixelFormat get_hw_format(AVCodecContext *ctx, const enum AVPixelFormat * } // namespace -FrameReader::FrameReader() {} +FrameReader::FrameReader() { + av_log_set_level(AV_LOG_QUIET); +} FrameReader::~FrameReader() { for (AVPacket *pkt : packets) { @@ -81,13 +84,13 @@ bool FrameReader::load(const std::byte *data, size_t size, bool no_cuda, std::at if (ret != 0) { char err_str[1024] = {0}; av_strerror(ret, err_str, std::size(err_str)); - printf("Error loading video - %s\n", err_str); + rError("Error loading video - %s", err_str); return false; } ret = avformat_find_stream_info(input_ctx, nullptr); if (ret < 0) { - printf("cannot find a video stream in the input file\n"); + rError("cannot find a video stream in the input file"); return false; } @@ -105,7 +108,7 @@ bool FrameReader::load(const std::byte *data, size_t size, bool no_cuda, std::at if (has_cuda_device && !no_cuda) { if (!initHardwareDecoder(AV_HWDEVICE_TYPE_CUDA)) { - printf("No CUDA capable device was found. fallback to CPU decoding.\n"); + rWarning("No CUDA capable device was found. fallback to CPU decoding."); } else { nv12toyuv_buffer.resize(getYUVSize()); } @@ -135,8 +138,8 @@ bool FrameReader::initHardwareDecoder(AVHWDeviceType hw_device_type) { for (int i = 0;; i++) { const AVCodecHWConfig *config = avcodec_get_hw_config(decoder_ctx->codec, i); if (!config) { - printf("decoder %s does not support hw device type %s.\n", - decoder_ctx->codec->name, av_hwdevice_get_type_name(hw_device_type)); + rWarning("decoder %s does not support hw device type %s.", decoder_ctx->codec->name, + av_hwdevice_get_type_name(hw_device_type)); return false; } if (config->methods & AV_CODEC_HW_CONFIG_METHOD_HW_DEVICE_CTX && config->device_type == hw_device_type) { @@ -149,7 +152,7 @@ bool FrameReader::initHardwareDecoder(AVHWDeviceType hw_device_type) { if (ret < 0) { hw_pix_fmt = AV_PIX_FMT_NONE; has_cuda_device = false; - printf("Failed to create specified HW device %d.\n", ret); + rWarning("Failed to create specified HW device %d.", ret); return false; } @@ -192,20 +195,21 @@ bool FrameReader::decode(int idx, uint8_t *rgb, uint8_t *yuv) { AVFrame *FrameReader::decodeFrame(AVPacket *pkt) { int ret = avcodec_send_packet(decoder_ctx, pkt); if (ret < 0) { - printf("Error sending a packet for decoding\n"); + rError("Error sending a packet for decoding: %d", ret); return nullptr; } av_frame_.reset(av_frame_alloc()); ret = avcodec_receive_frame(decoder_ctx, av_frame_.get()); if (ret != 0) { + rError("avcodec_receive_frame error: %d", ret); return nullptr; } if (av_frame_->format == hw_pix_fmt) { hw_frame.reset(av_frame_alloc()); if ((ret = av_hwframe_transfer_data(hw_frame.get(), av_frame_.get(), 0)) < 0) { - printf("error transferring the data from GPU to CPU\n"); + rError("error transferring the data from GPU to CPU"); return nullptr; } return hw_frame.get(); diff --git a/selfdrive/ui/replay/logreader.cc b/selfdrive/ui/replay/logreader.cc index d836ef11f87301..579fe50644dd3f 100644 --- a/selfdrive/ui/replay/logreader.cc +++ b/selfdrive/ui/replay/logreader.cc @@ -1,7 +1,6 @@ #include "selfdrive/ui/replay/logreader.h" #include -#include #include "selfdrive/ui/replay/util.h" Event::Event(const kj::ArrayPtr &amsg, bool frame) : reader(amsg), frame(frame) { @@ -59,7 +58,7 @@ bool LogReader::load(const std::byte *data, size_t size, std::atomic *abor raw_ = decompressBZ2(data, size, abort); if (raw_.empty()) { if (!(abort && *abort)) { - std::cout << "failed to decompress log" << std::endl; + rWarning("failed to decompress log"); } return false; } @@ -92,9 +91,9 @@ bool LogReader::load(const std::byte *data, size_t size, std::atomic *abor events.push_back(evt); } } catch (const kj::Exception &e) { - std::cout << "failed to parse log : " << e.getDescription().cStr() << std::endl; + rWarning("failed to parse log : %s", e.getDescription().cStr()); if (!events.empty()) { - std::cout << "read " << events.size() << " events from corrupt log" << std::endl; + rWarning("read %zu events from corrupt log", events.size()); } } diff --git a/selfdrive/ui/replay/main.cc b/selfdrive/ui/replay/main.cc index 180aecc7e13ee9..a7d2f54042ee1a 100644 --- a/selfdrive/ui/replay/main.cc +++ b/selfdrive/ui/replay/main.cc @@ -1,109 +1,13 @@ -#include - #include #include -#include -#include -#include -#include +#include "selfdrive/ui/replay/consoleui.h" #include "selfdrive/ui/replay/replay.h" const QString DEMO_ROUTE = "4cf7a6ad03080c90|2021-09-29--13-46-36"; -struct termios oldt = {}; -Replay *replay = nullptr; - -void sigHandler(int s) { - std::signal(s, SIG_DFL); - if (oldt.c_lflag) { - tcsetattr(STDIN_FILENO, TCSANOW, &oldt); - } - replay->stop(); - qApp->quit(); -} - -int getch() { - int ch; - struct termios newt; - - tcgetattr(STDIN_FILENO, &oldt); - newt = oldt; - newt.c_lflag &= ~(ICANON | ECHO); - - tcsetattr(STDIN_FILENO, TCSANOW, &newt); - ch = getchar(); - tcsetattr(STDIN_FILENO, TCSANOW, &oldt); - - return ch; -} - -void keyboardThread(Replay *replay_) { - char c; - while (true) { - c = getch(); - if (c == '\n') { - printf("Enter seek request: "); - std::string r; - std::cin >> r; - - try { - if (r[0] == '#') { - r.erase(0, 1); - replay_->seekTo(std::stoi(r) * 60, false); - } else { - replay_->seekTo(std::stoi(r), false); - } - } catch (std::invalid_argument) { - qDebug() << "invalid argument"; - } - getch(); // remove \n from entering seek - } else if (c == 'e') { - replay_->seekToFlag(FindFlag::nextEngagement); - } else if (c == 'd') { - replay_->seekToFlag(FindFlag::nextDisEngagement); - } else if (c == 'm') { - replay_->seekTo(+60, true); - } else if (c == 'M') { - replay_->seekTo(-60, true); - } else if (c == 's') { - replay_->seekTo(+10, true); - } else if (c == 'S') { - replay_->seekTo(-10, true); - } else if (c == 'G') { - replay_->seekTo(0, true); - } else if (c == 'x') { - if (replay_->hasFlag(REPLAY_FLAG_FULL_SPEED)) { - replay_->removeFlag(REPLAY_FLAG_FULL_SPEED); - qInfo() << "replay at normal speed"; - } else { - replay_->addFlag(REPLAY_FLAG_FULL_SPEED); - qInfo() << "replay at full speed"; - } - } else if (c == ' ') { - replay_->pause(!replay_->isPaused()); - } - } -} - -void replayMessageOutput(QtMsgType type, const QMessageLogContext &context, const QString &msg) { - QByteArray localMsg = msg.toLocal8Bit(); - if (type == QtDebugMsg) { - std::cout << "\033[38;5;248m" << localMsg.constData() << "\033[00m" << std::endl; - } else if (type == QtWarningMsg) { - std::cout << "\033[38;5;227m" << localMsg.constData() << "\033[00m" << std::endl; - } else if (type == QtCriticalMsg) { - std::cout << "\033[38;5;196m" << localMsg.constData() << "\033[00m" << std::endl; - } else { - std::cout << localMsg.constData() << std::endl; - } -} int main(int argc, char *argv[]) { - qInstallMessageHandler(replayMessageOutput); - QApplication app(argc, argv); - std::signal(SIGINT, sigHandler); - std::signal(SIGTERM, sigHandler); const std::tuple flags[] = { {"dcam", REPLAY_FLAG_DCAM, "load driver camera"}, @@ -145,16 +49,12 @@ int main(int argc, char *argv[]) { replay_flags |= flag; } } - replay = new Replay(route, allow, block, nullptr, replay_flags, parser.value("data_dir"), &app); + Replay *replay = new Replay(route, allow, block, nullptr, replay_flags, parser.value("data_dir"), &app); if (!replay->load()) { return 0; } - replay->start(parser.value("start").toInt()); - // start keyboard control thread - QThread *t = new QThread(); - QObject::connect(t, &QThread::started, [=]() { keyboardThread(replay); }); - QObject::connect(t, &QThread::finished, t, &QThread::deleteLater); - t->start(); + ConsoleUI console_ui(replay); + replay->start(parser.value("start").toInt()); return app.exec(); } diff --git a/selfdrive/ui/replay/replay.cc b/selfdrive/ui/replay/replay.cc index 98e211fb53fcbe..fd1a4b1990316c 100644 --- a/selfdrive/ui/replay/replay.cc +++ b/selfdrive/ui/replay/replay.cc @@ -1,7 +1,7 @@ #include "selfdrive/ui/replay/replay.h" -#include #include +#include #include #include "cereal/services.h" @@ -23,6 +23,7 @@ Replay::Replay(QString route, QStringList allow, QStringList block, SubMaster *s } } qDebug() << "services " << s; + qDebug() << "loading route " << route; if (sm == nullptr) { pm = std::make_unique(s); @@ -30,22 +31,16 @@ Replay::Replay(QString route, QStringList allow, QStringList block, SubMaster *s route_ = std::make_unique(route, data_dir); events_ = std::make_unique>(); new_events_ = std::make_unique>(); - - qRegisterMetaType("FindFlag"); - connect(this, &Replay::seekTo, this, &Replay::doSeek); - connect(this, &Replay::seekToFlag, this, &Replay::doSeekToFlag); - connect(this, &Replay::stop, this, &Replay::doStop); - connect(this, &Replay::segmentChanged, this, &Replay::queueSegment); } Replay::~Replay() { - doStop(); + stop(); } -void Replay::doStop() { +void Replay::stop() { if (!stream_thread_ && segments_.empty()) return; - qDebug() << "shutdown: in progress..."; + rInfo("shutdown: in progress..."); if (stream_thread_ != nullptr) { exit_ = updating_events_ = true; stream_cv_.notify_one(); @@ -55,12 +50,14 @@ void Replay::doStop() { } segments_.clear(); camera_server_.reset(nullptr); - qDebug() << "shutdown: done"; + timeline_future.waitForFinished(); + rInfo("shutdown: done"); } bool Replay::load() { if (!route_->load()) { - qCritical() << "failed to load route" << route_->name() << "from server"; + qCritical() << "failed to load route" << route_->name() + << "from" << (route_->dir().isEmpty() ? "server" : route_->dir()); return false; } @@ -75,7 +72,7 @@ bool Replay::load() { qCritical() << "no valid segments in route" << route_->name(); return false; } - qInfo() << "load route" << route_->name() << "with" << segments_.size() << "valid segments"; + rInfo("load route %s with %zu valid segments", qPrintable(route_->name()), segments_.size()); return true; } @@ -94,21 +91,17 @@ void Replay::updateEvents(const std::function &lambda) { stream_cv_.notify_one(); } -void Replay::doSeek(int seconds, bool relative) { - if (segments_.empty()) return; - +void Replay::seekTo(int seconds, bool relative) { + seconds = relative ? seconds + currentSeconds() : seconds; updateEvents([&]() { - if (relative) { - seconds += currentSeconds(); - } seconds = std::max(0, seconds); int seg = seconds / 60; if (segments_.find(seg) == segments_.end()) { - qWarning() << "can't seek to" << seconds << "s, segment" << seg << "is invalid"; + rWarning("can't seek to %d s segment %d is invalid", seconds, seg); return true; } - qInfo() << "seeking to" << seconds << "s, segment" << seg; + rInfo("seeking to %d s, segment %d", seconds, seg); current_segment_ = seg; cur_mono_time_ = route_start_ts_ + seconds * 1e9; return isSegmentMerged(seg); @@ -116,61 +109,68 @@ void Replay::doSeek(int seconds, bool relative) { queueSegment(); } -void Replay::doSeekToFlag(FindFlag flag) { - if (flag == FindFlag::nextEngagement) { - qInfo() << "seeking to the next engagement..."; - } else if (flag == FindFlag::nextDisEngagement) { - qInfo() << "seeking to the disengagement..."; +void Replay::seekToFlag(FindFlag flag) { + if (auto next = find(flag)) { + seekTo(*next - 2, false); // seek to 2 seconds before next } - - updateEvents([&]() { - if (auto next = find(flag)) { - uint64_t tm = *next - 2 * 1e9; // seek to 2 seconds before next - if (tm <= cur_mono_time_) { - return true; - } - - cur_mono_time_ = tm; - current_segment_ = currentSeconds() / 60; - return isSegmentMerged(current_segment_); - } else { - qWarning() << "seeking failed"; - return true; - } - }); - - queueSegment(); } -std::optional Replay::find(FindFlag flag) { - // Search in all segments - for (const auto &[n, _] : segments_) { - if (n < current_segment_) continue; +void Replay::buildTimeline() { + uint64_t engaged_begin = 0; + uint64_t alert_begin = 0; + TimelineType alert_type = TimelineType::None; + for (int i = 0; i < segments_.size() && !exit_; ++i) { LogReader log; - bool cache_to_local = true; // cache qlog to local for fast seek - if (!log.load(route_->at(n).qlog.toStdString(), nullptr, cache_to_local, 0, 3)) continue; + if (!log.load(route_->at(i).qlog.toStdString(), &exit_, !hasFlag(REPLAY_FLAG_NO_FILE_CACHE), 0, 3)) continue; for (const Event *e : log.events) { - if (e->mono_time > cur_mono_time_ && e->which == cereal::Event::Which::CONTROLS_STATE) { - const auto cs = e->event.getControlsState(); - if (flag == FindFlag::nextEngagement && cs.getEnabled()) { - return e->mono_time; - } else if (flag == FindFlag::nextDisEngagement && !cs.getEnabled()) { - return e->mono_time; + if (e->which == cereal::Event::Which::CONTROLS_STATE) { + auto cs = e->event.getControlsState(); + + if (!engaged_begin && cs.getEnabled()) { + engaged_begin = e->mono_time; + } else if (engaged_begin && !cs.getEnabled()) { + std::lock_guard lk(timeline_lock); + timeline.push_back({toSeconds(engaged_begin), toSeconds(e->mono_time), TimelineType::Engaged}); + engaged_begin = 0; + } + + if (!alert_begin && cs.getAlertType().size() > 0) { + alert_begin = e->mono_time; + alert_type = TimelineType::AlertInfo; + if (cs.getAlertStatus() != cereal::ControlsState::AlertStatus::NORMAL) { + alert_type = cs.getAlertStatus() == cereal::ControlsState::AlertStatus::USER_PROMPT + ? TimelineType::AlertWarning + : TimelineType::AlertCritical; + } + } else if (alert_begin && cs.getAlertType().size() == 0) { + std::lock_guard lk(timeline_lock); + timeline.push_back({toSeconds(alert_begin), toSeconds(e->mono_time), alert_type}); + alert_begin = 0; } } } } +} + +std::optional Replay::find(FindFlag flag) { + int cur_ts = currentSeconds(); + for (auto [start_ts, end_ts, type] : getTimeline()) { + if (type == TimelineType::Engaged) { + if (flag == FindFlag::nextEngagement && start_ts > cur_ts) { + return start_ts; + } else if (flag == FindFlag::nextDisEngagement && end_ts > cur_ts) { + return end_ts; + } + } + } return std::nullopt; } void Replay::pause(bool pause) { updateEvents([=]() { - qInfo() << (pause ? "paused..." : "resuming"); - if (pause) { - qInfo() << "at " << currentSeconds() << "s"; - } + rWarning("%s at %d s", pause ? "paused..." : "resuming", currentSeconds()); paused_ = pause; return true; }); @@ -178,14 +178,14 @@ void Replay::pause(bool pause) { void Replay::setCurrentSegment(int n) { if (current_segment_.exchange(n) != n) { - emit segmentChanged(); + QMetaObject::invokeMethod(this, &Replay::queueSegment, Qt::QueuedConnection); } } void Replay::segmentLoadFinished(bool success) { if (!success) { Segment *seg = qobject_cast(sender()); - qWarning() << "failed to load segment " << seg->seg_num << ", removing it from current replay list"; + rWarning("failed to load segment %d, removing it from current replay list", seg->seg_num); segments_.erase(seg->seg_num); } queueSegment(); @@ -201,19 +201,18 @@ void Replay::queueSegment() { } // load one segment at a time for (auto it = cur; it != end; ++it) { - if (!it->second) { - if (it == cur || std::prev(it)->second->isLoaded()) { - auto &[n, seg] = *it; + auto &[n, seg] = *it; + if ((seg && !seg->isLoaded()) || !seg) { + if (!seg) { + rDebug("loading segment %d...", n); seg = std::make_unique(n, route_->at(n), flags_); QObject::connect(seg.get(), &Segment::loadFinished, this, &Replay::segmentLoadFinished); - qDebug() << "loading segment" << n << "..."; } break; } } - const auto &cur_segment = cur->second; - enableHttpLogging(!cur_segment->isLoaded()); + const auto &cur_segment = cur->second; // merge the previous adjacent segment if it's loaded auto begin = segments_.find(cur_segment->seg_num - 1); if (begin == segments_.end() || !(begin->second && begin->second->isLoaded())) { @@ -228,6 +227,7 @@ void Replay::queueSegment() { // start stream thread if (stream_thread_ == nullptr && cur_segment->isLoaded()) { startStream(cur_segment.get()); + emit streamStarted(); } } @@ -235,13 +235,18 @@ void Replay::mergeSegments(const SegmentMap::iterator &begin, const SegmentMap:: // merge 3 segments in sequence. std::vector segments_need_merge; size_t new_events_size = 0; - for (auto it = begin; it != end && it->second->isLoaded() && segments_need_merge.size() < 3; ++it) { + for (auto it = begin; it != end && it->second && it->second->isLoaded() && segments_need_merge.size() < 3; ++it) { segments_need_merge.push_back(it->first); new_events_size += it->second->log->events.size(); } if (segments_need_merge != segments_merged_) { - qDebug() << "merge segments" << segments_need_merge; + std::string s; + for (int i = 0; i < segments_need_merge.size(); ++i) { + s += std::to_string(segments_need_merge[i]); + if (i != segments_need_merge.size() - 1) s += ", "; + } + rDebug("merge segments %s", s.c_str()); new_events_->clear(); new_events_->reserve(new_events_size); for (int n : segments_need_merge) { @@ -269,10 +274,11 @@ void Replay::startStream(const Segment *cur_segment) { // write CarParams it = std::find_if(events.begin(), events.end(), [](auto e) { return e->which == cereal::Event::Which::CAR_PARAMS; }); if (it != events.end()) { + car_fingerprint_ = (*it)->event.getCarParams().getCarFingerprint(); auto bytes = (*it)->bytes(); Params().put("CarParams", (const char *)bytes.begin(), bytes.size()); } else { - qWarning() << "failed to read CarParams from current segment"; + rWarning("failed to read CarParams from current segment"); } // start camera server @@ -291,6 +297,8 @@ void Replay::startStream(const Segment *cur_segment) { QObject::connect(stream_thread_, &QThread::started, [=]() { stream(); }); QObject::connect(stream_thread_, &QThread::finished, stream_thread_, &QThread::deleteLater); stream_thread_->start(); + + timeline_future = QtConcurrent::run(this, &Replay::buildTimeline); } void Replay::publishMessage(const Event *e) { @@ -298,7 +306,7 @@ void Replay::publishMessage(const Event *e) { auto bytes = e->bytes(); int ret = pm->send(sockets_[e->which], (capnp::byte *)bytes.begin(), bytes.size()); if (ret == -1) { - qDebug() << "stop publishing" << sockets_[e->which] << "due to multiple publishers error"; + rWarning("stop publishing %s due to multiple publishers error", sockets_[e->which]); sockets_[e->which] = nullptr; } } else { @@ -324,9 +332,7 @@ void Replay::publishFrame(const Event *e) { } void Replay::stream() { - float last_print = 0; cereal::Event::Which cur_which = cereal::Event::Which::INIT_DATA; - std::unique_lock lk(stream_lock_); while (true) { @@ -337,7 +343,7 @@ void Replay::stream() { Event cur_event(cur_which, cur_mono_time_); auto eit = std::upper_bound(events_->begin(), events_->end(), &cur_event, Event::lessThan()); if (eit == events_->end()) { - qDebug() << "waiting for events..."; + rInfo("waiting for events..."); continue; } @@ -348,12 +354,7 @@ void Replay::stream() { const Event *evt = (*eit); cur_which = evt->which; cur_mono_time_ = evt->mono_time; - const int current_ts = currentSeconds(); - if (last_print > current_ts || (current_ts - last_print) > 5.0) { - last_print = current_ts; - qInfo() << "at " << current_ts << "s"; - } - setCurrentSegment(current_ts / 60); + setCurrentSegment(toSeconds(cur_mono_time_) / 60); // migration for pandaState -> pandaStates to keep UI working for old segments if (cur_which == cereal::Event::Which::PANDA_STATE_D_E_P_R_E_C_A_T_E_D) { @@ -396,8 +397,8 @@ void Replay::stream() { if (eit == events_->end() && !hasFlag(REPLAY_FLAG_NO_LOOP)) { int last_segment = segments_.rbegin()->first; if (current_segment_ >= last_segment && isSegmentMerged(last_segment)) { - qInfo() << "reaches the end of route, restart from beginning"; - emit seekTo(0, false); + rInfo("reaches the end of route, restart from beginning"); + QMetaObject::invokeMethod(this, std::bind(&Replay::seekTo, this, 0, false), Qt::QueuedConnection); } } } diff --git a/selfdrive/ui/replay/replay.h b/selfdrive/ui/replay/replay.h index 4f979905065f84..0fc171ba2df8e3 100644 --- a/selfdrive/ui/replay/replay.h +++ b/selfdrive/ui/replay/replay.h @@ -28,6 +28,8 @@ enum class FindFlag { nextDisEngagement }; +enum class TimelineType { None, Engaged, AlertInfo, AlertWarning, AlertCritical }; + class Replay : public QObject { Q_OBJECT @@ -37,23 +39,28 @@ class Replay : public QObject { ~Replay(); bool load(); void start(int seconds = 0); + void stop(); void pause(bool pause); - bool isPaused() const { return paused_; } + void seekToFlag(FindFlag flag); + void seekTo(int seconds, bool relative); + inline bool isPaused() const { return paused_; } inline bool hasFlag(REPLAY_FLAGS flag) const { return flags_ & flag; } inline void addFlag(REPLAY_FLAGS flag) { flags_ |= flag; } inline void removeFlag(REPLAY_FLAGS flag) { flags_ &= ~flag; } + inline const Route* route() const { return route_.get(); } + inline int currentSeconds() const { return (cur_mono_time_ - route_start_ts_) / 1e9; } + inline int toSeconds(uint64_t mono_time) const { return (mono_time - route_start_ts_) / 1e9; } + inline int totalSeconds() const { return segments_.size() * 60; } + inline const std::string &carFingerprint() const { return car_fingerprint_; } + inline const std::vector> getTimeline() { + std::lock_guard lk(timeline_lock); + return timeline; + } signals: - void segmentChanged(); - void seekTo(int seconds, bool relative); - void seekToFlag(FindFlag flag); - void stop(); + void streamStarted(); protected slots: - void queueSegment(); - void doStop(); - void doSeek(int seconds, bool relative); - void doSeekToFlag(FindFlag flag); void segmentLoadFinished(bool sucess); protected: @@ -62,11 +69,12 @@ protected slots: void startStream(const Segment *cur_segment); void stream(); void setCurrentSegment(int n); + void queueSegment(); void mergeSegments(const SegmentMap::iterator &begin, const SegmentMap::iterator &end); void updateEvents(const std::function& lambda); void publishMessage(const Event *e); void publishFrame(const Event *e); - inline int currentSeconds() const { return (cur_mono_time_ - route_start_ts_) / 1e9; } + void buildTimeline(); inline bool isSegmentMerged(int n) { return std::find(segments_merged_.begin(), segments_merged_.end(), n) != segments_merged_.end(); } @@ -80,7 +88,7 @@ protected slots: std::atomic current_segment_ = 0; SegmentMap segments_; // the following variables must be protected with stream_lock_ - bool exit_ = false; + std::atomic exit_ = false; bool paused_ = false; bool events_updated_ = false; uint64_t route_start_ts_ = 0; @@ -96,4 +104,9 @@ protected slots: std::unique_ptr route_; std::unique_ptr camera_server_; std::atomic flags_ = REPLAY_FLAG_NONE; + + std::mutex timeline_lock; + QFuture timeline_future; + std::vector> timeline; + std::string car_fingerprint_; }; diff --git a/selfdrive/ui/replay/route.cc b/selfdrive/ui/replay/route.cc index fe6e21a91ad820..e6a6177149adf7 100644 --- a/selfdrive/ui/replay/route.cc +++ b/selfdrive/ui/replay/route.cc @@ -26,7 +26,7 @@ RouteIdentifier Route::parseRoute(const QString &str) { bool Route::load() { if (route_.str.isEmpty()) { - qInfo() << "invalid route format"; + rInfo("invalid route format"); return false; } return data_dir_.isEmpty() ? loadFromServer() : loadFromLocal(); diff --git a/selfdrive/ui/replay/route.h b/selfdrive/ui/replay/route.h index c39eef7d92ef4c..ac8fba75ca3288 100644 --- a/selfdrive/ui/replay/route.h +++ b/selfdrive/ui/replay/route.h @@ -27,6 +27,7 @@ class Route { Route(const QString &route, const QString &data_dir = {}); bool load(); inline const QString &name() const { return route_.str; } + inline const QString &dir() const { return data_dir_; } inline const RouteIdentifier &identifier() const { return route_; } inline const std::map &segments() const { return segments_; } inline const SegmentFile &at(int n) { return segments_.at(n); } diff --git a/selfdrive/ui/replay/tests/test_replay.cc b/selfdrive/ui/replay/tests/test_replay.cc index 6063dfe7d57fec..3cafa9b534ed49 100644 --- a/selfdrive/ui/replay/tests/test_replay.cc +++ b/selfdrive/ui/replay/tests/test_replay.cc @@ -1,3 +1,6 @@ +#include +#include + #include #include @@ -10,6 +13,16 @@ const QString DEMO_ROUTE = "4cf7a6ad03080c90|2021-09-29--13-46-36"; const std::string TEST_RLOG_URL = "https://commadataci.blob.core.windows.net/openpilotci/0c94aa1e1296d7c6/2021-05-05--19-48-37/0/rlog.bz2"; const std::string TEST_RLOG_CHECKSUM = "5b966d4bb21a100a8c4e59195faeb741b975ccbe268211765efd1763d892bfb3"; +bool donload_to_file(const std::string &url, const std::string &local_file, int chunk_size = 5 * 1024 * 1024, int retries = 3) { + do { + if (httpDownload(url, local_file, chunk_size)) { + return true; + } + std::this_thread::sleep_for(std::chrono::milliseconds(500)); + } while (--retries >= 0); + return false; +} + TEST_CASE("httpMultiPartDownload") { char filename[] = "/tmp/XXXXXX"; close(mkstemp(filename)); @@ -17,16 +30,13 @@ TEST_CASE("httpMultiPartDownload") { const size_t chunk_size = 5 * 1024 * 1024; std::string content; SECTION("download to file") { - bool ret = false; - for (int i = 0; i < 3 && !ret; ++i) { - ret = httpDownload(TEST_RLOG_URL, filename, chunk_size); - } - REQUIRE(ret); + REQUIRE(donload_to_file(TEST_RLOG_URL, filename, chunk_size)); content = util::read_file(filename); } SECTION("download to buffer") { for (int i = 0; i < 3 && content.empty(); ++i) { content = httpGet(TEST_RLOG_URL, chunk_size); + std::this_thread::sleep_for(std::chrono::milliseconds(500)); } REQUIRE(!content.empty()); } @@ -67,14 +77,9 @@ TEST_CASE("LogReader") { } } -TEST_CASE("Segment") { - auto flags = GENERATE(REPLAY_FLAG_DCAM | REPLAY_FLAG_ECAM, REPLAY_FLAG_QCAMERA); - Route demo_route(DEMO_ROUTE); - REQUIRE(demo_route.load()); - REQUIRE(demo_route.segments().size() == 11); - +void read_segment(int n, const SegmentFile &segment_file, uint32_t flags) { QEventLoop loop; - Segment segment(0, demo_route.at(0), flags); + Segment segment(n, segment_file, flags); QObject::connect(&segment, &Segment::loadFinished, [&]() { REQUIRE(segment.isLoaded() == true); REQUIRE(segment.log != nullptr); @@ -99,8 +104,8 @@ TEST_CASE("Segment") { } std::unique_ptr rgb_buf = std::make_unique(fr->getRGBSize()); std::unique_ptr yuv_buf = std::make_unique(fr->getYUVSize()); - // sequence get 50 frames - for (int i = 0; i < 50; ++i) { + // sequence get 100 frames + for (int i = 0; i < 100; ++i) { REQUIRE(fr->get(i, rgb_buf.get(), yuv_buf.get())); } } @@ -110,6 +115,43 @@ TEST_CASE("Segment") { loop.exec(); } +TEST_CASE("Route") { + // Create a local route from remote for testing + Route remote_route(DEMO_ROUTE); + REQUIRE(remote_route.load()); + char tmp_path[] = "/tmp/root_XXXXXX"; + const std::string data_dir = mkdtemp(tmp_path); + const std::string route_name = DEMO_ROUTE.mid(17).toStdString(); + for (int i = 0; i < 2; ++i) { + std::string log_path = util::string_format("%s/%s--%d/", data_dir.c_str(), route_name.c_str(), i); + util::create_directories(log_path, 0755); + REQUIRE(donload_to_file(remote_route.at(i).rlog.toStdString(), log_path + "rlog.bz2")); + REQUIRE(donload_to_file(remote_route.at(i).road_cam.toStdString(), log_path + "fcamera.hevc")); + REQUIRE(donload_to_file(remote_route.at(i).driver_cam.toStdString(), log_path + "dcamera.hevc")); + REQUIRE(donload_to_file(remote_route.at(i).wide_road_cam.toStdString(), log_path + "ecamera.hevc")); + REQUIRE(donload_to_file(remote_route.at(i).qcamera.toStdString(), log_path + "qcamera.ts")); + } + + SECTION("Local route") { + auto flags = GENERATE(REPLAY_FLAG_DCAM | REPLAY_FLAG_ECAM, REPLAY_FLAG_QCAMERA); + Route route(DEMO_ROUTE, QString::fromStdString(data_dir)); + REQUIRE(route.load()); + REQUIRE(route.segments().size() == 2); + for (int i = 0; i < route.segments().size(); ++i) { + read_segment(i, route.at(i), flags); + } + }; + SECTION("Remote route") { + auto flags = GENERATE(REPLAY_FLAG_DCAM | REPLAY_FLAG_ECAM, REPLAY_FLAG_QCAMERA); + Route route(DEMO_ROUTE); + REQUIRE(route.load()); + REQUIRE(route.segments().size() == 11); + for (int i = 0; i < 2; ++i) { + read_segment(i, route.at(i), flags); + } + }; +} + // helper class for unit tests class TestReplay : public Replay { public: diff --git a/selfdrive/ui/replay/util.cc b/selfdrive/ui/replay/util.cc index d6eba2e2a87018..513cd965be0c76 100644 --- a/selfdrive/ui/replay/util.cc +++ b/selfdrive/ui/replay/util.cc @@ -15,6 +15,40 @@ #include "selfdrive/common/timing.h" #include "selfdrive/common/util.h" +ReplayMessageHandler message_handler = nullptr; +DownloadProgressHandler download_progress_handler = nullptr; + +void installMessageHandler(ReplayMessageHandler handler) { message_handler = handler; } +void installDownloadProgressHandler(DownloadProgressHandler handler) { download_progress_handler = handler; } + +void logMessage(ReplyMsgType type, const char *fmt, ...) { + static std::mutex lock; + std::lock_guard lk(lock); + + char *msg_buf = nullptr; + va_list args; + va_start(args, fmt); + int ret = vasprintf(&msg_buf, fmt, args); + va_end(args); + if (ret <= 0 || !msg_buf) return; + + if (message_handler) { + message_handler(type, msg_buf); + } else { + if (type == ReplyMsgType::Debug) { + std::cout << "\033[38;5;248m" << msg_buf << "\033[00m" << std::endl; + } else if (type == ReplyMsgType::Warning) { + std::cout << "\033[38;5;227m" << msg_buf << "\033[00m" << std::endl; + } else if (type == ReplyMsgType::Critical) { + std::cout << "\033[38;5;196m" << msg_buf << "\033[00m" << std::endl; + } else { + std::cout << msg_buf << std::endl; + } + } + + free(msg_buf); +} + namespace { struct CURLGlobalInitializer { @@ -23,7 +57,6 @@ struct CURLGlobalInitializer { }; static CURLGlobalInitializer curl_initializer; -static std::atomic enable_http_logging = false; template struct MultiPartWriter { @@ -57,6 +90,38 @@ size_t write_cb(char *data, size_t size, size_t count, void *userp) { size_t dumy_write_cb(char *data, size_t size, size_t count, void *userp) { return size * count; } +struct DownloadStats { + void add(const std::string &url, uint64_t total_bytes) { + std::lock_guard lk(lock); + items[url] = {0, total_bytes}; + } + + void remove(const std::string &url) { + std::lock_guard lk(lock); + items.erase(url); + } + + void update(const std::string &url, uint64_t downloaded, bool success = true) { + std::lock_guard lk(lock); + items[url].first = downloaded; + + auto stat = std::accumulate(items.begin(), items.end(), std::pair{}, [=](auto &a, auto &b){ + return std::pair{a.first + b.second.first, a.second + b.second.second}; + }); + double tm = millis_since_boot(); + if (download_progress_handler && ((tm - prev_tm) > 500 || !success || stat.first >= stat.second)) { + download_progress_handler(stat.first, stat.second, success); + prev_tm = tm; + } + } + + std::mutex lock; + std::map> items; + double prev_tm = 0; +}; + +} // namespace + std::string formattedDataSize(size_t size) { if (size < 1024) { return std::to_string(size) + " B"; @@ -67,8 +132,6 @@ std::string formattedDataSize(size_t size) { } } -} // namespace - size_t getRemoteFileSize(const std::string &url, std::atomic *abort) { CURL *curl = curl_easy_init(); if (!curl) return -1; @@ -99,12 +162,11 @@ std::string getUrlWithoutQuery(const std::string &url) { return (idx == std::string::npos ? url : url.substr(0, idx)); } -void enableHttpLogging(bool enable) { - enable_http_logging = enable; -} - template bool httpDownload(const std::string &url, T &buf, size_t chunk_size, size_t content_length, std::atomic *abort) { + static DownloadStats download_stats; + download_stats.add(url, content_length); + int parts = 1; if (chunk_size > 0 && content_length > 10 * 1024 * 1024) { parts = std::nearbyint(content_length / (float)chunk_size); @@ -134,23 +196,11 @@ bool httpDownload(const std::string &url, T &buf, size_t chunk_size, size_t cont curl_multi_add_handle(cm, eh); } - size_t prev_written = 0; - double last_print = millis_since_boot(); int still_running = 1; while (still_running > 0 && !(abort && *abort)) { curl_multi_wait(cm, nullptr, 0, 1000, nullptr); curl_multi_perform(cm, &still_running); - - if (enable_http_logging) { - if (double ts = millis_since_boot(); (ts - last_print) > 2 * 1000) { - size_t average = (written - prev_written) / ((ts - last_print) / 1000.); - int progress = std::min(100, 100.0 * (double)written / (double)content_length); - std::cout << "downloading " << getUrlWithoutQuery(url) << " - " << progress - << "% (" << formattedDataSize(average) << "/s)" << std::endl; - last_print = ts; - prev_written = written; - } - } + download_stats.update(url, written); } CURLMsg *msg; @@ -164,21 +214,25 @@ bool httpDownload(const std::string &url, T &buf, size_t chunk_size, size_t cont if (res_status == 206) { complete++; } else { - std::cout << "Download failed: http error code: " << res_status << std::endl; + rWarning("Download failed: http error code: %d", res_status); } } else { - std::cout << "Download failed: connection failure: " << msg->data.result << std::endl; + rWarning("Download failed: connection failure: %d", msg->data.result); } } } + bool success = complete == parts; + download_stats.update(url, written, success); + download_stats.remove(url); + for (const auto &[e, w] : writers) { curl_multi_remove_handle(cm, e); curl_easy_cleanup(e); } curl_multi_cleanup(cm); - return complete == parts; + return success; } std::string httpGet(const std::string &url, size_t chunk_size, std::atomic *abort) { @@ -221,7 +275,7 @@ std::string decompressBZ2(const std::byte *in, size_t in_size, std::atomic if (bzerror == BZ_OK && prev_write_pos == strm.next_out) { // content is corrupt bzerror = BZ_STREAM_END; - std::cout << "decompressBZ2 error : content is corrupt" << std::endl; + rWarning("decompressBZ2 error : content is corrupt"); break; } diff --git a/selfdrive/ui/replay/util.h b/selfdrive/ui/replay/util.h index cd4b179cdce41d..6c808095e84b64 100644 --- a/selfdrive/ui/replay/util.h +++ b/selfdrive/ui/replay/util.h @@ -1,14 +1,34 @@ #pragma once #include +#include #include +enum class ReplyMsgType { + Info, + Debug, + Warning, + Critical +}; + +typedef std::function ReplayMessageHandler; +void installMessageHandler(ReplayMessageHandler); +void logMessage(ReplyMsgType type, const char* fmt, ...); + +#define rInfo(fmt, ...) ::logMessage(ReplyMsgType::Info, fmt, ## __VA_ARGS__) +#define rDebug(fmt, ...) ::logMessage(ReplyMsgType::Debug, fmt, ## __VA_ARGS__) +#define rWarning(fmt, ...) ::logMessage(ReplyMsgType::Warning, fmt, ## __VA_ARGS__) +#define rError(fmt, ...) ::logMessage(ReplyMsgType::Critical , fmt, ## __VA_ARGS__) + std::string sha256(const std::string &str); void precise_nano_sleep(long sleep_ns); std::string decompressBZ2(const std::string &in, std::atomic *abort = nullptr); std::string decompressBZ2(const std::byte *in, size_t in_size, std::atomic *abort = nullptr); -void enableHttpLogging(bool enable); std::string getUrlWithoutQuery(const std::string &url); size_t getRemoteFileSize(const std::string &url, std::atomic *abort = nullptr); std::string httpGet(const std::string &url, size_t chunk_size = 0, std::atomic *abort = nullptr); + +typedef std::function DownloadProgressHandler; +void installDownloadProgressHandler(DownloadProgressHandler); bool httpDownload(const std::string &url, const std::string &file, size_t chunk_size = 0, std::atomic *abort = nullptr); +std::string formattedDataSize(size_t size); diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index dd64a4263b5281..c615ce7358a637 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -130,12 +130,6 @@ static void update_state(UIState *s) { SubMaster &sm = *(s->sm); UIScene &scene = s->scene; - if (sm.updated("modelV2")) { - update_model(s, sm["modelV2"].getModelV2()); - } - if (sm.updated("radarState") && sm.rcv_frame("modelV2") >= s->scene.started_frame) { - update_leads(s, sm["radarState"].getRadarState(), sm["modelV2"].getModelV2().getPosition()); - } if (sm.updated("liveCalibration")) { auto rpy_list = sm["liveCalibration"].getLiveCalibration().getRpyCalib(); Eigen::Vector3d rpy; @@ -152,6 +146,14 @@ static void update_state(UIState *s) { } } } + if (s->worldObjectsVisible()) { + if (sm.updated("modelV2")) { + update_model(s, sm["modelV2"].getModelV2()); + } + if (sm.updated("radarState") && sm.rcv_frame("modelV2") >= s->scene.started_frame) { + update_leads(s, sm["radarState"].getRadarState(), sm["modelV2"].getModelV2().getPosition()); + } + } if (sm.updated("pandaStates")) { auto pandaStates = sm["pandaStates"].getPandaStates(); if (pandaStates.size() > 0) { @@ -185,16 +187,22 @@ static void update_state(UIState *s) { } } } - if (sm.updated("roadCameraState")) { + if (!Hardware::TICI() && sm.updated("roadCameraState")) { auto camera_state = sm["roadCameraState"].getRoadCameraState(); float max_lines = Hardware::EON() ? 5408 : 1904; float max_gain = Hardware::EON() ? 1.0: 10.0; float max_ev = max_lines * max_gain; - if (Hardware::TICI()) { - max_ev /= 6; - } + float ev = camera_state.getGain() * float(camera_state.getIntegLines()); + + scene.light_sensor = std::clamp(1.0 - (ev / max_ev), 0.0, 1.0); + } else if (Hardware::TICI() && sm.updated("wideRoadCameraState")) { + auto camera_state = sm["wideRoadCameraState"].getWideRoadCameraState(); + + float max_lines = 1904; + float max_gain = 10.0; + float max_ev = max_lines * max_gain / 6; float ev = camera_state.getGain() * float(camera_state.getIntegLines()); @@ -240,7 +248,8 @@ void UIState::updateStatus() { UIState::UIState(QObject *parent) : QObject(parent) { sm = std::make_unique>({ "modelV2", "controlsState", "liveCalibration", "radarState", "deviceState", "roadCameraState", - "pandaStates", "carParams", "driverMonitoringState", "sensorEvents", "carState", "liveLocationKalman", "sentryState", + "pandaStates", "carParams", "driverMonitoringState", "sensorEvents", "carState", "liveLocationKalman", + "wideRoadCameraState", "sentryState", }); std::string toyota_distance_btn = util::read_file("/data/community/params/toyota_distance_btn"); if (toyota_distance_btn == "true") { diff --git a/selfdrive/version.py b/selfdrive/version.py index 6c9664dafeb09d..3a3f63839b7b55 100644 --- a/selfdrive/version.py +++ b/selfdrive/version.py @@ -55,12 +55,24 @@ def get_origin(default: Optional[str] = None) -> Optional[str]: return run_cmd_default(["git", "config", "--get", "remote.origin.url"], default=default) +@cache +def get_normalized_origin(default: Optional[str] = None) -> Optional[str]: + return get_origin()\ + .replace("git@", "", 1)\ + .replace(".git", "", 1)\ + .replace("https://", "", 1)\ + .replace(":", "/", 1) + + @cache def get_version() -> str: with open(os.path.join(os.path.dirname(os.path.abspath(__file__)), "common", "version.h")) as _versionf: version = _versionf.read().split('"')[1] return version +@cache +def get_short_version() -> str: + return get_version().split('-')[0] @cache def is_prebuilt() -> bool: @@ -69,6 +81,8 @@ def is_prebuilt() -> bool: @cache def is_comma_remote() -> bool: + # note to fork maintainers, this is used for release metrics. please do not + # touch this to get rid of the orange startup alert. there's better ways to do that origin = get_origin() if origin is None: return False @@ -113,11 +127,6 @@ def is_dirty(fork=False) -> bool: pass dirty = (subprocess.call(["git", "diff-index", "--quiet", branch, "--"]) != 0) - - correct_remote = is_fork_remote() if fork else is_comma_remote() - dirty = dirty or (not correct_remote) - dirty = dirty or ('master' in branch) - except subprocess.CalledProcessError: cloudlog.exception("git subprocess failed while checking dirty") dirty = True @@ -134,7 +143,9 @@ def is_dirty(fork=False) -> bool: print(f"Dirty: {is_dirty()}") print(f"Version: {get_version()}") + print(f"Short version: {get_short_version()}") print(f"Origin: {get_origin()}") + print(f"Normalized origin: {get_normalized_origin()}") print(f"Branch: {get_branch()}") print(f"Short branch: {get_short_branch()}") print(f"Prebuilt: {is_prebuilt()}") diff --git a/tools/joystick/README.md b/tools/joystick/README.md index 33366d1efd52c6..5a45785db6eb48 100644 --- a/tools/joystick/README.md +++ b/tools/joystick/README.md @@ -1,13 +1,17 @@ # Joystick -**Hardware needed**: [comma two devkit](https://comma.ai/shop/products/comma-two-devkit), laptop, joystick (optional) +**Hardware needed**: device running openpilot, laptop, joystick (optional) -With joystickd, you can connect your laptop to your comma device over the network and debug controls using a joystick or keyboard, however a joystick is recommended for more precise control. +With joystickd, you can connect your laptop to your comma device over the network and debug controls using a joystick or keyboard. +joystickd uses [inputs](https://pypi.org/project/inputs) which supports many common gamepads and joysticks. -Using a keyboard ---- +## Usage + +The car must be off, and openpilot must be offroad before starting `joystickd`. + +### Using a keyboard -To get started, ssh into your comma device and start joystickd with the following command: +SSH into your comma device and start joystickd with the following command: ```shell tools/joystick/joystickd.py --keyboard @@ -15,22 +19,26 @@ tools/joystick/joystickd.py --keyboard The available buttons and axes will print showing their key mappings. In general, the WASD keys control gas and brakes and steering torque in 5% increments. -Using a joystick ---- +### Joystick on your comma three + +Plug the joystick into your comma three aux USB-C port. Then, SSH into the device and start `joystickd.py`. + +### Joystick on your laptop -In order to use a joystick over the network, we need to run joystickd locally from your laptop and have it send `testJoystick` ZMQ packets over the network to the comma device. First connect a compatible joystick to your PC; joystickd uses [inputs](https://pypi.org/project/inputs) which supports many common gamepads and joysticks. +In order to use a joystick over the network, we need to run joystickd locally from your laptop and have it send `testJoystick` packets over the network to the comma device. -1. Connect your laptop to your comma device's hotspot and open a new ssh shell. Since joystickd is being run on your laptop, we need to write a parameter to let controlsd know to start in joystick debug mode: +1. Connect a joystick to your PC. +2. Connect your laptop to your comma device's hotspot and open a new SSH shell. Since joystickd is being run on your laptop, we need to write a parameter to let controlsd know to start in joystick debug mode: ```shell # on your comma device echo -n "1" > /data/params/d/JoystickDebugMode ``` -2. Run bridge with your laptop's IP address. This republishes the `testJoystick` packets sent from your laptop so that openpilot can receive them: +3. Run bridge with your laptop's IP address. This republishes the `testJoystick` packets sent from your laptop so that openpilot can receive them: ```shell # on your comma device cereal/messaging/bridge {LAPTOP_IP} testJoystick ``` -3. Finally, start joystickd on your laptop and tell it to publish ZMQ packets over the network: +4. Start joystickd on your laptop in ZMQ mode. ```shell # on your laptop export ZMQ=1 @@ -38,8 +46,8 @@ In order to use a joystick over the network, we need to run joystickd locally fr ``` --- -Now start your car and openpilot should go into debug mode with an alert on startup! The status of the axes will display on the alert, while button statuses print in the shell. +Now start your car and openpilot should go into joystick mode with an alert on startup! The status of the axes will display on the alert, while button statuses print in the shell. Make sure the conditions are met in the panda to allow controls (e.g. cruise control engaged). You can also make a modification to the panda code to always allow controls. -![Imgur](steer.gif) +![](steer.gif) diff --git a/tools/joystick/joystickd.py b/tools/joystick/joystickd.py index 0e3269556ab4c2..5a86293441fac2 100755 --- a/tools/joystick/joystickd.py +++ b/tools/joystick/joystickd.py @@ -1,10 +1,11 @@ #!/usr/bin/env python +import os import argparse +from inputs import get_gamepad import cereal.messaging as messaging from common.numpy_fast import interp, clip from common.params import Params -from inputs import get_gamepad from tools.lib.kbhit import KBHit @@ -62,7 +63,7 @@ def joystick_thread(use_keyboard): joystick = Keyboard() if use_keyboard else Joystick() while True: - ret = joystick.update() # processes joystick/key events and handles state of axes + ret = joystick.update() if ret: dat = messaging.new_message('testJoystick') dat.testJoystick.axes = [joystick.axes_values[a] for a in joystick.axes_values] @@ -72,19 +73,24 @@ def joystick_thread(use_keyboard): if __name__ == '__main__': - parser = argparse.ArgumentParser( - description='Publishes events from your joystick to control your car', - formatter_class=argparse.ArgumentDefaultsHelpFormatter) + parser = argparse.ArgumentParser(description='Publishes events from your joystick to control your car.\n' + + 'openpilot must be offroad before starting joysticked.', + formatter_class=argparse.ArgumentDefaultsHelpFormatter) parser.add_argument('--keyboard', action='store_true', help='Use your keyboard instead of a joystick') args = parser.parse_args() + if not Params().get_bool("IsOffroad") and "ZMQ" not in os.environ: + print("The car must be off before running joystickd.") + exit() + + print() if args.keyboard: - print('\nGas/brake control: `W` and `S` keys\n' - 'Steering control: `A` and `D` keys') - print('Buttons:\n' - '- `R`: Resets axes\n' - '- `C`: Cancel cruise control') + print('Gas/brake control: `W` and `S` keys') + print('Steering control: `A` and `D` keys') + print('Buttons') + print('- `R`: Resets axes') + print('- `C`: Cancel cruise control') else: - print('\nUsing joystick, make sure to run bridge on your device if running over the network!') + print('Using joystick, make sure to run cereal/messaging/bridge on your device if running over the network!') joystick_thread(args.keyboard) diff --git a/tools/lib/bootlog.py b/tools/lib/bootlog.py new file mode 100644 index 00000000000000..dc1cf35c52f340 --- /dev/null +++ b/tools/lib/bootlog.py @@ -0,0 +1,48 @@ +import datetime +import functools +import re + +from tools.lib.auth_config import get_token +from tools.lib.api import CommaApi +from tools.lib.helpers import RE, timestamp_to_datetime + + +@functools.total_ordering +class Bootlog: + def __init__(self, url: str): + self._url = url + + r = re.search(RE.BOOTLOG_NAME, url) + if not r: + raise Exception(f"Unable to parse: {url}") + + self._dongle_id = r.group('dongle_id') + self._timestamp = r.group('timestamp') + + @property + def url(self) -> str: + return self._url + + @property + def dongle_id(self) -> str: + return self._dongle_id + + @property + def timestamp(self) -> str: + return self._timestamp + + @property + def datetime(self) -> datetime.datetime: + return timestamp_to_datetime(self._timestamp) + + def __eq__(self, b) -> bool: + return self.datetime == b.datetime + + def __lt__(self, b) -> bool: + return self.datetime < b.datetime + + +def get_bootlogs(dongle_id: str): + api = CommaApi(get_token()) + r = api.get(f'v1/devices/{dongle_id}/bootlogs') + return [Bootlog(b) for b in r] diff --git a/tools/lib/filereader.py b/tools/lib/filereader.py index 3d4c46b2200a51..4ea9ad4290a184 100644 --- a/tools/lib/filereader.py +++ b/tools/lib/filereader.py @@ -1,6 +1,11 @@ +import os from tools.lib.url_file import URLFile +DATA_ENDPOINT = os.getenv("DATA_ENDPOINT", "http://data-raw.internal/") + def FileReader(fn, debug=False): + if fn.startswith("cd:/"): + fn = fn.replace("cd:/", DATA_ENDPOINT) if fn.startswith("http://") or fn.startswith("https://"): return URLFile(fn, debug=debug) return open(fn, "rb") diff --git a/tools/lib/framereader.py b/tools/lib/framereader.py index ac5962462ee688..bdf48bb43834d9 100644 --- a/tools/lib/framereader.py +++ b/tools/lib/framereader.py @@ -17,10 +17,7 @@ from tools.lib.exceptions import DataUnreadableError from common.file_helpers import atomic_write_in_dir -try: - from xx.chffr.lib.filereader import FileReader -except ImportError: - from tools.lib.filereader import FileReader +from tools.lib.filereader import FileReader HEVC_SLICE_B = 0 HEVC_SLICE_P = 1 diff --git a/tools/lib/helpers.py b/tools/lib/helpers.py new file mode 100644 index 00000000000000..efe704b9ecfc89 --- /dev/null +++ b/tools/lib/helpers.py @@ -0,0 +1,20 @@ +import datetime + +TIME_FMT = "%Y-%m-%d--%H-%M-%S" + +# regex patterns +class RE: + DONGLE_ID = r'(?P[a-z0-9]{16})' + TIMESTAMP = r'(?P[0-9]{4}-[0-9]{2}-[0-9]{2}--[0-9]{2}-[0-9]{2}-[0-9]{2})' + ROUTE_NAME = r'{}[|_/]{}'.format(DONGLE_ID, TIMESTAMP) + SEGMENT_NAME = r'{}(?:--|/)(?P[0-9]+)'.format(ROUTE_NAME) + BOOTLOG_NAME = ROUTE_NAME + + EXPLORER_FILE = r'^(?P{})--(?P[a-z]+\.[a-z0-9]+)$'.format(SEGMENT_NAME) + OP_SEGMENT_DIR = r'^(?P{})$'.format(SEGMENT_NAME) + +def timestamp_to_datetime(t: str) -> datetime.datetime: + """ + Convert an openpilot route timestamp to a python datetime + """ + return datetime.datetime.strptime(t, TIME_FMT) diff --git a/tools/lib/logreader.py b/tools/lib/logreader.py index fc7a8dcb70f51d..c8d506b4b3ed64 100755 --- a/tools/lib/logreader.py +++ b/tools/lib/logreader.py @@ -5,10 +5,7 @@ import urllib.parse import capnp -try: - from xx.chffr.lib.filereader import FileReader -except ImportError: - from tools.lib.filereader import FileReader +from tools.lib.filereader import FileReader from cereal import log as capnp_log # this is an iterator itself, and uses private variables from LogReader diff --git a/tools/lib/route.py b/tools/lib/route.py index c87bd04b9ebcdd..8092096a0ba6bb 100644 --- a/tools/lib/route.py +++ b/tools/lib/route.py @@ -6,11 +6,7 @@ from tools.lib.auth_config import get_token from tools.lib.api import CommaApi - -ROUTE_NAME_RE = r'(?P[a-z0-9]{16})[|_/](?P[0-9]{4}-[0-9]{2}-[0-9]{2}--[0-9]{2}-[0-9]{2}-[0-9]{2})' -SEGMENT_NAME_RE = r'{}(?:--|/)(?P[0-9]+)'.format(ROUTE_NAME_RE) -EXPLORER_FILE_RE = r'^(?P{})--(?P[a-z]+\.[a-z0-9]+)$'.format(SEGMENT_NAME_RE) -OP_SEGMENT_DIR_RE = r'^(?P{})$'.format(SEGMENT_NAME_RE) +from tools.lib.helpers import RE QLOG_FILENAMES = ['qlog.bz2'] QCAMERA_FILENAMES = ['qcamera.ts'] @@ -100,8 +96,8 @@ def _get_segments_local(self, data_dir): for f in files: fullpath = os.path.join(data_dir, f) - explorer_match = re.match(EXPLORER_FILE_RE, f) - op_match = re.match(OP_SEGMENT_DIR_RE, f) + explorer_match = re.match(RE.EXPLORER_FILE, f) + op_match = re.match(RE.OP_SEGMENT_DIR, f) if explorer_match: segment_name = explorer_match.group('segment_name') diff --git a/tools/plotjuggler/juggle.py b/tools/plotjuggler/juggle.py index 7cafaf60acf8e0..1dcbcae0c9ca19 100755 --- a/tools/plotjuggler/juggle.py +++ b/tools/plotjuggler/juggle.py @@ -23,6 +23,7 @@ DEMO_ROUTE = "4cf7a6ad03080c90|2021-09-29--13-46-36" RELEASES_URL="https://github.com/commaai/PlotJuggler/releases/download/latest" INSTALL_DIR = os.path.join(juggle_dir, "bin") +PLOTJUGGLER_BIN = os.path.join(juggle_dir, "bin/plotjuggler") def install(): @@ -70,7 +71,7 @@ def start_juggler(fn=None, dbc=None, layout=None): if layout is not None: extra_args += f" -l {layout}" - cmd = f'plotjuggler --plugin_folders {INSTALL_DIR}{extra_args}' + cmd = f'{PLOTJUGGLER_BIN} --plugin_folders {INSTALL_DIR}{extra_args}' subprocess.call(cmd, shell=True, env=env, cwd=juggle_dir) @@ -149,6 +150,10 @@ def juggle_route(route_or_segment_name, segment_count, qlog, can, layout): install() sys.exit() + if not os.path.exists(PLOTJUGGLER_BIN): + print("PlotJuggler is missing. Downloading...") + install() + if args.stream: start_juggler(layout=args.layout) else: diff --git a/tools/plotjuggler/layouts/controls_mismatch_debug.xml b/tools/plotjuggler/layouts/controls_mismatch_debug.xml new file mode 100644 index 00000000000000..e50b5ffb3407d2 --- /dev/null +++ b/tools/plotjuggler/layouts/controls_mismatch_debug.xml @@ -0,0 +1,46 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/tools/sim/Dockerfile.sim b/tools/sim/Dockerfile.sim index f6cf0d9b0b7922..4ffd5bea14216d 100644 --- a/tools/sim/Dockerfile.sim +++ b/tools/sim/Dockerfile.sim @@ -42,7 +42,7 @@ RUN pip install --upgrade pip && \ # get same tmux config used on NEOS for debugging RUN cd $HOME && \ - wget https://raw.githubusercontent.com/commaai/eon-neos-builder/master/devices/eon/home/.tmux.conf + curl -O https://raw.githubusercontent.com/commaai/eon-neos-builder/master/devices/eon/home/.tmux.conf ENV PYTHONPATH $HOME/openpilot:${PYTHONPATH} RUN mkdir -p $HOME/openpilot diff --git a/tools/sim/lib/can.py b/tools/sim/lib/can.py index 4873947d4b9f37..af0f415bbc1c97 100755 --- a/tools/sim/lib/can.py +++ b/tools/sim/lib/can.py @@ -12,11 +12,11 @@ def get_car_can_parser(): dbc_f = 'honda_civic_touring_2016_can_generated' signals = [ - ("STEER_TORQUE", 0xe4, 0), - ("STEER_TORQUE_REQUEST", 0xe4, 0), - ("COMPUTER_BRAKE", 0x1fa, 0), - ("COMPUTER_BRAKE_REQUEST", 0x1fa, 0), - ("GAS_COMMAND", 0x200, 0), + ("STEER_TORQUE", 0xe4), + ("STEER_TORQUE_REQUEST", 0xe4), + ("COMPUTER_BRAKE", 0x1fa), + ("COMPUTER_BRAKE_REQUEST", 0x1fa), + ("GAS_COMMAND", 0x200), ] checks = [ (0xe4, 100), diff --git a/tools/ubuntu_setup.sh b/tools/ubuntu_setup.sh index 44ddb4f014a0a0..e688c79c12be86 100755 --- a/tools/ubuntu_setup.sh +++ b/tools/ubuntu_setup.sh @@ -13,12 +13,12 @@ function install_ubuntu_common_requirements() { sudo apt-get install -y --no-install-recommends \ autoconf \ build-essential \ + ca-certificates \ clang \ cmake \ make \ cppcheck \ libtool \ - libstdc++-arm-none-eabi-newlib \ gcc-arm-none-eabi \ bzip2 \ liblzma-dev \ @@ -28,7 +28,6 @@ function install_ubuntu_common_requirements() { libcapnp-dev \ curl \ libcurl4-openssl-dev \ - wget \ git \ git-lfs \ ffmpeg \ @@ -50,13 +49,6 @@ function install_ubuntu_common_requirements() { libsqlite3-dev \ libusb-1.0-0-dev \ libzmq3-dev \ - libsdl1.2-dev \ - libsdl-image1.2-dev \ - libsdl-mixer1.2-dev \ - libsdl-ttf2.0-dev \ - libsmpeg-dev \ - libportmidi-dev \ - libfreetype6-dev \ libsystemd-dev \ locales \ opencl-headers \ @@ -64,10 +56,8 @@ function install_ubuntu_common_requirements() { ocl-icd-opencl-dev \ clinfo \ python-dev \ - python3-pip \ qml-module-qtquick2 \ qtmultimedia5-dev \ - qtwebengine5-dev \ qtlocation5-dev \ qtpositioning5-dev \ libqt5sql5-sqlite \ diff --git a/tools/webcam/Dockerfile b/tools/webcam/Dockerfile index 7de86a4a7be6f8..70317cd92483a3 100644 --- a/tools/webcam/Dockerfile +++ b/tools/webcam/Dockerfile @@ -27,7 +27,7 @@ RUN apt-get update && apt-get install -y --no-install-recommends \ mkdir /tmp/opencv_build && \ cd /tmp/opencv_build && \ - wget https://github.com/opencv/opencv/archive/${OPENCV_VERSION}.tar.gz && \ + curl -L -O https://github.com/opencv/opencv/archive/${OPENCV_VERSION}.tar.gz && \ tar -xvf ${OPENCV_VERSION}.tar.gz && \ mv opencv-${OPENCV_VERSION} OpenCV && \ cd OpenCV && mkdir build && cd build && \ @@ -37,4 +37,4 @@ RUN apt-get update && apt-get install -y --no-install-recommends \ make install && \ ldconfig && \ - cd /tmp && rm -rf /tmp/opencv_build + cd / && rm -rf /tmp/*