diff --git a/.gitattributes b/.gitattributes index 1618dbf519..7a21b223d0 100644 --- a/.gitattributes +++ b/.gitattributes @@ -1,8 +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 -*.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 f04eaad86f..aef9a0a49a 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/.pre-commit-config.yaml b/.pre-commit-config.yaml index 65b60d95ce..3fc5596568 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 a3a2fc36f9..d7edeb1173 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/RELEASES.md b/RELEASES.md index efbd5bf91c..592ad822f1 100644 --- a/RELEASES.md +++ b/RELEASES.md @@ -1,10 +1,14 @@ 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! + * Mazda CX-5 2022 support thanks to Jafaral! * 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 42542ee96c..03860ae0b2 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit 42542ee96ca00744e6117d57533defe6f01ba14d +Subproject commit 03860ae0b2b8128cae7768e4301d889e627c9275 diff --git a/common/params_pyx.pyx b/common/params_pyx.pyx index 7b35e815df..a8c43c2f2f 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 0b3d8dbda0..906198871b 100644 --- a/common/tests/test_params.py +++ b/common/tests/test_params.py @@ -24,14 +24,6 @@ class TestParams(unittest.TestCase): 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 1f641a0109..063f326acd 100644 --- a/docs/CARS.md +++ b/docs/CARS.md @@ -43,8 +43,9 @@ | 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 | | Toyota | Camry 2018-20 | All | Stock | 0mph4 | 0mph | | Toyota | Camry 2021-22 | All | openpilot | 0mph4 | 0mph | @@ -63,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 | @@ -78,99 +80,96 @@ ## 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-5 2022 | All | 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 cb0a0572d0..95e0f0c8ac 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/pyextra/acados_template/acados_ocp_solver_pyx.pyx b/pyextra/acados_template/acados_ocp_solver_pyx.pyx index b950d880b0..9bcf5bb46a 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 f2b10004b9..faf6a41db7 100644 --- a/release/files_common +++ b/release/files_common @@ -70,7 +70,7 @@ selfdrive/version.py selfdrive/__init__.py selfdrive/config.py -selfdrive/crash.py +selfdrive/sentry.py selfdrive/swaglog.py selfdrive/logmessaged.py selfdrive/tombstoned.py @@ -88,6 +88,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 @@ -198,6 +200,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 @@ -228,17 +232,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 @@ -255,6 +261,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 @@ -549,7 +556,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 @@ -571,8 +578,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 @@ -589,22 +594,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_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 a0c6a0c844..275e336846 100755 --- a/selfdrive/athena/athenad.py +++ b/selfdrive/athena/athenad.py @@ -14,7 +14,7 @@ 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 @@ -55,7 +55,7 @@ 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(): @@ -128,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(): @@ -145,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 diff --git a/selfdrive/athena/registration.py b/selfdrive/athena/registration.py index f19540eb87..10f5d46f77 100755 --- a/selfdrive/athena/registration.py +++ b/selfdrive/athena/registration.py @@ -10,13 +10,18 @@ from common.params import Params 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 06d762c180..fb5936dde1 100755 --- a/selfdrive/athena/tests/test_athenad.py +++ b/selfdrive/athena/tests/test_athenad.py @@ -166,6 +166,31 @@ class TestAthenadMethods(unittest.TestCase): 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 07ded56e1b..922107509a 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 1178b4cafa..677132584b 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(0); // 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); @@ -453,7 +457,9 @@ void panda_state_thread(PubMaster *pm, std::vector pandas, bool spoofin for (const auto &panda : pandas) { panda->send_heartbeat(engaged); } - util::sleep_for(500); + + uint64_t dt = nanos_since_boot() - start_time; + util::sleep_for(500 - dt / 1000000ULL); } } @@ -552,38 +558,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; @@ -599,12 +578,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"); @@ -620,22 +593,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; diff --git a/selfdrive/boardd/boardd.h b/selfdrive/boardd/boardd.h new file mode 100644 index 0000000000..d3c9e1f94a --- /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 0000000000..d802e42f86 --- /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 6850ad41b2..f38101f49f 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 1a18a7f15a..0e88bb3850 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 424ad78ab3..b313f74e7e 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 75bd79bfdf..f53f06dceb 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 030bbe47e1..e86de9ffd6 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 f06bd341df..668410d6f7 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 0f14a23794..358d93d759 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 0704bc8950..90bfdd8a93 100644 --- a/selfdrive/car/car_helpers.py +++ b/selfdrive/car/car_helpers.py @@ -125,7 +125,7 @@ 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) diff --git a/selfdrive/car/chrysler/carstate.py b/selfdrive/car/chrysler/carstate.py index 66cd178f88..30a0bf6a3a 100644 --- a/selfdrive/car/chrysler/carstate.py +++ b/selfdrive/car/chrysler/carstate.py @@ -75,33 +75,33 @@ class CarState(CarStateBase): @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 @@ class CarState(CarStateBase): 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/interface.py b/selfdrive/car/chrysler/interface.py index a893b222f8..aead037815 100755 --- a/selfdrive/car/chrysler/interface.py +++ b/selfdrive/car/chrysler/interface.py @@ -60,8 +60,7 @@ class CarInterface(CarInterfaceBase): 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 3139efad34..5b8dba67f4 100755 --- a/selfdrive/car/chrysler/radar_interface.py +++ b/selfdrive/car/chrysler/radar_interface.py @@ -11,30 +11,25 @@ NUMBER_MSGS = len(RADAR_MSGS_C) + len(RADAR_MSGS_D) 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 e32de19501..3fd32e9bc7 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/carstate.py b/selfdrive/car/ford/carstate.py index d71e65352f..eba623f5ce 100644 --- a/selfdrive/car/ford/carstate.py +++ b/selfdrive/car/ford/carstate.py @@ -39,20 +39,20 @@ class CarState(CarStateBase): @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/interface.py b/selfdrive/car/ford/interface.py index 12cf6367e3..0faaa3f669 100755 --- a/selfdrive/car/ford/interface.py +++ b/selfdrive/car/ford/interface.py @@ -21,7 +21,7 @@ class CarInterface(CarInterfaceBase): 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 diff --git a/selfdrive/car/ford/radar_interface.py b/selfdrive/car/ford/radar_interface.py index 20a435b082..94eb8bb0cc 100755 --- a/selfdrive/car/ford/radar_interface.py +++ b/selfdrive/car/ford/radar_interface.py @@ -7,12 +7,12 @@ from selfdrive.car.interfaces import RadarInterfaceBase 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 7c6cc6d2df..e1e2206472 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/gm/carcontroller.py b/selfdrive/car/gm/carcontroller.py index 56c7c2a3ca..7ad1e7cf88 100644 --- a/selfdrive/car/gm/carcontroller.py +++ b/selfdrive/car/gm/carcontroller.py @@ -27,7 +27,7 @@ class CarController(): 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 @@ class CarController(): 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 @@ class CarController(): # 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 diff --git a/selfdrive/car/gm/carstate.py b/selfdrive/car/gm/carstate.py index 5180d2e511..134bd9d075 100644 --- a/selfdrive/car/gm/carstate.py +++ b/selfdrive/car/gm/carstate.py @@ -3,8 +3,7 @@ from common.numpy_fast import mean 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 @@ class CarState(CarStateBase): @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 @@ class CarState(CarStateBase): ] 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 45fb55098e..d6a2d3cfee 100755 --- a/selfdrive/car/gm/interface.py +++ b/selfdrive/car/gm/interface.py @@ -47,7 +47,7 @@ class CarInterface(CarInterfaceBase): # 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), @@ -226,7 +226,7 @@ class CarInterface(CarInterfaceBase): # 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 d1ad1c1635..66fac54748 100755 --- a/selfdrive/car/gm/radar_interface.py +++ b/selfdrive/car/gm/radar_interface.py @@ -14,6 +14,7 @@ NUM_SLOTS = 20 # 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 d475b4bd67..8e09076713 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 @@ FINGERPRINTS = { } 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 33d01f61b6..49581799a7 100644 --- a/selfdrive/car/honda/carcontroller.py +++ b/selfdrive/car/honda/carcontroller.py @@ -117,7 +117,7 @@ class CarController(): 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 @@ class CarController(): # 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 029413ad72..d1907a9d22 100644 --- a/selfdrive/car/honda/carstate.py +++ b/selfdrive/car/honda/carstate.py @@ -11,34 +11,33 @@ TransmissionType = car.CarParams.TransmissionType 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 @@ class CarState(CarStateBase): 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 @@ -207,9 +203,7 @@ class CarState(CarStateBase): # LOW_SPEED_LOCKOUT is not worth a warning 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 @@ class CarState(CarStateBase): 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 @@ class CarState(CarStateBase): 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 @@ class CarState(CarStateBase): ] 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 @@ class CarState(CarStateBase): @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 62dbb24c34..94e4305909 100755 --- a/selfdrive/car/honda/interface.py +++ b/selfdrive/car/honda/interface.py @@ -47,7 +47,6 @@ class CarInterface(CarInterfaceBase): 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 @@ class CarInterface(CarInterfaceBase): 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 diff --git a/selfdrive/car/honda/radar_interface.py b/selfdrive/car/honda/radar_interface.py index 45e015af6e..629ab01d4c 100755 --- a/selfdrive/car/honda/radar_interface.py +++ b/selfdrive/car/honda/radar_interface.py @@ -4,13 +4,13 @@ from opendbc.can.parser import CANParser 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 cbe2369b14..6e347b8da3 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 @@ FW_VERSIONS = { 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 @@ FW_VERSIONS = { 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 @@ FW_VERSIONS = { 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 @@ DBC = { 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 @@ STEER_THRESHOLD = { 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 6adefd26c6..f7c43bd6e3 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -46,7 +46,7 @@ class CarController(): 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 @@ class CarController(): 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 @@ class CarController(): 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) diff --git a/selfdrive/car/hyundai/carstate.py b/selfdrive/car/hyundai/carstate.py index e889f24fc9..bdd49e2067 100644 --- a/selfdrive/car/hyundai/carstate.py +++ b/selfdrive/car/hyundai/carstate.py @@ -120,57 +120,57 @@ class CarState(CarStateBase): @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 @@ class CarState(CarStateBase): 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 @@ class CarState(CarStateBase): 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 @@ class CarState(CarStateBase): ] 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/interface.py b/selfdrive/car/hyundai/interface.py index 89e1934cdf..463ff3569a 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -347,7 +347,7 @@ class CarInterface(CarInterfaceBase): 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 a7269e9283..0d22611fb5 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) @@ -81,7 +81,7 @@ class RadarInterface(RadarInterfaceBase): 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 1b55bde3dd..d940144f86 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 @@ -672,7 +670,7 @@ FW_VERSIONS = { 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 @@ FW_VERSIONS = { 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 @@ FW_VERSIONS = { 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', diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index 089dc2045d..afd7091194 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -72,6 +72,7 @@ class CarInterfaceBase(ABC): def get_std_params(candidate, fingerprint): ret = car.CarParams.new_message() ret.carFingerprint = candidate + ret.unsafeMode = 0 # see safety_declarations.h for allowed values # standard ALC params ret.steerControlType = car.CarParams.SteerControlType.torque @@ -97,6 +98,7 @@ class CarInterfaceBase(ABC): ret.longitudinalTuning.kiV = [1.] ret.longitudinalActuatorDelayLowerBound = 0.15 ret.longitudinalActuatorDelayUpperBound = 0.15 + ret.steerLimitTimer = 1.0 return ret @abstractmethod @@ -107,7 +109,7 @@ class CarInterfaceBase(ABC): 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: @@ -152,9 +154,7 @@ class CarInterfaceBase(ABC): 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 (cs_out.gasPressed and (not self.CS.out.gasPressed) and cs_out.vEgo > gas_resume_speed) or \ + if (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 c65ff72ed5..60bb620377 100644 --- a/selfdrive/car/mazda/carcontroller.py +++ b/selfdrive/car/mazda/carcontroller.py @@ -19,7 +19,7 @@ class CarController(): 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 7ebf60c96b..feb1147549 100644 --- a/selfdrive/car/mazda/carstate.py +++ b/selfdrive/car/mazda/carstate.py @@ -16,6 +16,7 @@ class CarState(CarStateBase): 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): @@ -64,12 +65,13 @@ class CarState(CarStateBase): # Either due to low speed or hands off lkas_blocked = cp.vl["STEER_RATE"]["LKAS_BLOCK"] == 1 - # LKAS is enabled at 52kph going up and disabled at 45kph going down - # wait for LKAS_BLOCK signal to clear when going up since it lags behind the speed sometimes - if speed_kph > LKAS_LIMITS.ENABLE_SPEED and not lkas_blocked: - self.lkas_allowed_speed = True - elif speed_kph < LKAS_LIMITS.DISABLE_SPEED: - self.lkas_allowed_speed = False + if self.CP.minSteerSpeed > 0: + # LKAS is enabled at 52kph going up and disabled at 45kph going down + # wait for LKAS_BLOCK signal to clear when going up since it lags behind the speed sometimes + if speed_kph > LKAS_LIMITS.ENABLE_SPEED and not lkas_blocked: + self.lkas_allowed_speed = True + elif speed_kph < LKAS_LIMITS.DISABLE_SPEED: + self.lkas_allowed_speed = False # TODO: the signal used for available seems to be the adaptive cruise signal, instead of the main on # it should be used for carState.cruiseState.nonAdaptive instead @@ -86,34 +88,35 @@ class CarState(CarStateBase): # Check if LKAS is disabled due to lack of driver torque when all other states indicate # it should be enabled (steer lockout). Don't warn until we actually get lkas active # and lose it again, i.e, after initial lkas activation - ret.steerWarning = self.lkas_allowed_speed and lkas_blocked 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 @@ class CarState(CarStateBase): 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 @@ class CarState(CarStateBase): 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 a4c0ce705d..fb8edd6f42 100755 --- a/selfdrive/car/mazda/interface.py +++ b/selfdrive/car/mazda/interface.py @@ -22,14 +22,14 @@ class CarInterface(CarInterfaceBase): 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.CX5_2022, CAR.CX9_2021) ret.steerActuatorDelay = 0.1 ret.steerRateCost = 1.0 ret.steerLimitTimer = 0.8 tire_stiffness_factor = 0.70 # not optimized yet - if candidate == CAR.CX5: + if candidate in (CAR.CX5, CAR.CX5_2022): ret.mass = 3655 * CV.LB_TO_KG + STD_CARGO_KG ret.wheelbase = 2.7 ret.steerRatio = 15.5 @@ -58,8 +58,8 @@ class CarInterface(CarInterfaceBase): ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.19], [0.019]] ret.lateralTuning.pid.kf = 0.00006 - # No steer below disable speed - ret.minSteerSpeed = LKAS_LIMITS.DISABLE_SPEED * CV.KPH_TO_MS + if candidate not in (CAR.CX5_2022, ): + ret.minSteerSpeed = LKAS_LIMITS.DISABLE_SPEED * CV.KPH_TO_MS ret.centerToFront = ret.wheelbase * 0.41 @@ -86,7 +86,9 @@ class CarInterface(CarInterfaceBase): # 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 f18c30176c..bb4c63a151 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 @@ -21,7 +19,8 @@ class CAR: CX9 = "MAZDA CX-9" MAZDA3 = "MAZDA 3" MAZDA6 = "MAZDA 6" - CX9_2021 = "MAZDA CX-9 2021" # No Steer Lockout + CX9_2021 = "MAZDA CX-9 2021" + CX5_2022 = "MAZDA CX-5 2022" class LKAS_LIMITS: STEER_THRESHOLD = 15 @@ -37,6 +36,26 @@ class Buttons: FW_VERSIONS = { + CAR.CX5_2022 : { + (Ecu.eps, 0x730, None): [ + b'KSD5-3210X-C-00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.engine, 0x7e0, None): [ + b'PX2G-188K2-H\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.fwdRadar, 0x764, None): [ + b'K131-67XK2-F\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.esp, 0x760, None): [ + b'KSD5-437K2-A\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.fwdCamera, 0x706, None): [ + b'GSH7-67XK2-S\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.transmission, 0x7e1, None): [ + b'PYB2-21PS1-H\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + }, CAR.CX5: { (Ecu.eps, 0x730, None): [ b'KJ01-3210X-G-00\x00\x00\x00\x00\x00\x00\x00\x00\x00', @@ -244,10 +263,8 @@ DBC = { CAR.MAZDA3: dbc_dict('mazda_2017', None), CAR.MAZDA6: dbc_dict('mazda_2017', None), CAR.CX9_2021: dbc_dict('mazda_2017', None), + CAR.CX5_2022: dbc_dict('mazda_2017', None), } # Gen 1 hardware: same CAN messages and same camera -GEN1 = {CAR.CX5, CAR.CX9, CAR.CX9_2021, CAR.MAZDA3, CAR.MAZDA6} - -# Cars with a steering lockout -STEER_LOCKOUT_CAR = {CAR.CX5, CAR.CX9, CAR.MAZDA3, CAR.MAZDA6} +GEN1 = {CAR.CX5, CAR.CX9, CAR.CX9_2021, CAR.MAZDA3, CAR.MAZDA6, CAR.CX5_2022} diff --git a/selfdrive/car/nissan/carcontroller.py b/selfdrive/car/nissan/carcontroller.py index 40dd5da42a..8b30c11249 100644 --- a/selfdrive/car/nissan/carcontroller.py +++ b/selfdrive/car/nissan/carcontroller.py @@ -18,22 +18,20 @@ class CarController(): 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 - 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) diff --git a/selfdrive/car/nissan/carstate.py b/selfdrive/car/nissan/carstate.py index 4c605395d4..6b030e9b45 100644 --- a/selfdrive/car/nissan/carstate.py +++ b/selfdrive/car/nissan/carstate.py @@ -119,27 +119,26 @@ class CarState(CarStateBase): @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 = [ @@ -155,26 +154,26 @@ class CarState(CarStateBase): 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 += [ @@ -185,17 +184,17 @@ class CarState(CarStateBase): 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 @@ class CarState(CarStateBase): 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 @@ class CarState(CarStateBase): ] 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 @@ class CarState(CarStateBase): 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 @@ class CarState(CarStateBase): ] 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 = [ @@ -345,20 +340,11 @@ class CarState(CarStateBase): checks = [] if CP.carFingerprint in (CAR.ROGUE, CAR.XTRAIL): - signals += [ - ("CRUISE_ON", "PRO_PILOT", 0), - ] - checks += [ - ("PRO_PILOT", 100), - ] + 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 4350fb5447..c32fb13780 100644 --- a/selfdrive/car/nissan/interface.py +++ b/selfdrive/car/nissan/interface.py @@ -16,7 +16,7 @@ class CarInterface(CarInterfaceBase): 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 @@ -79,7 +79,7 @@ class CarInterface(CarInterfaceBase): 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 22bd4a1295..f7001d1417 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 @@ FW_VERSIONS = { (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 72b07f9192..afc91f4755 100644 --- a/selfdrive/car/subaru/carcontroller.py +++ b/selfdrive/car/subaru/carcontroller.py @@ -15,7 +15,7 @@ class CarController(): 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 @@ class CarController(): 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 b6eb54287f..1eefb18584 100644 --- a/selfdrive/car/subaru/carstate.py +++ b/selfdrive/car/subaru/carstate.py @@ -77,28 +77,27 @@ class CarState(CarStateBase): @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 @@ class CarState(CarStateBase): 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 @@ class CarState(CarStateBase): ("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 += [ @@ -164,26 +157,26 @@ class CarState(CarStateBase): 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 @@ class CarState(CarStateBase): ] 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 6c8659b4c0..8c6d188643 100644 --- a/selfdrive/car/subaru/interface.py +++ b/selfdrive/car/subaru/interface.py @@ -123,7 +123,7 @@ class CarInterface(CarInterfaceBase): 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 5ec95b0076..4e17453663 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 @@ FW_VERSIONS = { 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 @@ FW_VERSIONS = { 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 @@ FW_VERSIONS = { 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 @@ FW_VERSIONS = { 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 0b58632f0a..03f09f2407 100644 --- a/selfdrive/car/tesla/carcontroller.py +++ b/selfdrive/car/tesla/carcontroller.py @@ -12,12 +12,12 @@ class CarController(): 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 @@ -65,4 +65,4 @@ class CarController(): 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 39869baeae..51ae43ad1b 100644 --- a/selfdrive/car/tesla/carstate.py +++ b/selfdrive/car/tesla/carstate.py @@ -96,64 +96,64 @@ class CarState(CarStateBase): @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 @@ class CarState(CarStateBase): @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 e8d6ab6854..03012bc52e 100755 --- a/selfdrive/car/tesla/interface.py +++ b/selfdrive/car/tesla/interface.py @@ -40,6 +40,7 @@ class CarInterface(CarInterfaceBase): 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 @@ -70,6 +71,6 @@ class CarInterface(CarInterfaceBase): 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 f5ad12ba7e..a09f53e758 100755 --- a/selfdrive/car/tesla/radar_interface.py +++ b/selfdrive/car/tesla/radar_interface.py @@ -11,9 +11,9 @@ NUM_POINTS = len(RADAR_MSGS_A) 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 90bc45c7a5..616933789e 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 73ea2f350f..87ba0055f0 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -26,7 +26,7 @@ class CarController(): 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): @@ -49,7 +49,7 @@ class CarController(): 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): + if not active or CS.steer_state in (9, 25): apply_steer = 0 apply_steer_req = 0 else: @@ -106,7 +106,7 @@ class CarController(): 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 diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py index f7d353e691..df555abf46 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/selfdrive/car/toyota/carstate.py @@ -6,7 +6,7 @@ from opendbc.can.can_define import CANDefine 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): @@ -14,6 +14,7 @@ class CarState(CarStateBase): 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. @@ -28,9 +29,9 @@ class CarState(CarStateBase): 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 @@ -38,7 +39,9 @@ class CarState(CarStateBase): 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( @@ -72,11 +75,11 @@ class CarState(CarStateBase): 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) @@ -124,87 +127,88 @@ class CarState(CarStateBase): @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), + # 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"), ] 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), ("STEER_TORQUE_SENSOR", 50), ] + 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", 0)) - signals.append(("SET_SPEED", "DSU_CRUISE", 0)) + 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)) # 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)) return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 0) @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 @@ -214,7 +218,7 @@ class CarState(CarStateBase): ] if CP.carFingerprint in TSS2_CAR: - signals.append(("ACC_TYPE", "ACC_CONTROL", 0)) + signals.append(("ACC_TYPE", "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 old mode 100755 new mode 100644 index 22080985db..65c2faacab --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -2,7 +2,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 @@ -20,17 +20,15 @@ class CarInterface(CarInterfaceBase): 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 - 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 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 @@ -40,6 +38,14 @@ class CarInterface(CarInterfaceBase): set_lat_tune(ret.lateralTuning, LatTunes.INDI_PRIUS) ret.steerActuatorDelay = 0.3 + 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, 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 @@ -49,8 +55,6 @@ class CarInterface(CarInterfaceBase): set_lat_tune(ret.lateralTuning, LatTunes.LQR_RAV4) elif candidate == CAR.COROLLA: - ret.safetyConfigs[0].safetyParam = 88 - stop_and_go = False ret.wheelbase = 2.70 ret.steerRatio = 18.27 tire_stiffness_factor = 0.444 # not optimized yet @@ -98,8 +102,7 @@ class CarInterface(CarInterfaceBase): ret.mass = 4607. * CV.LB_TO_KG + STD_CARGO_KG # mean between normal and hybrid limited set_lat_tune(ret.lateralTuning, LatTunes.PID_G) - elif candidate in (CAR.AVALON, CAR.AVALON_2019, CAR.AVALONH_2019): - 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 @@ -129,20 +132,12 @@ class CarInterface(CarInterfaceBase): ret.mass = 3060. * CV.LB_TO_KG + STD_CARGO_KG set_lat_tune(ret.lateralTuning, 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, 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, LatTunes.PID_D) elif candidate == CAR.SIENNA: @@ -153,26 +148,14 @@ class CarInterface(CarInterfaceBase): ret.mass = 4590. * CV.LB_TO_KG + STD_CARGO_KG set_lat_tune(ret.lateralTuning, 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, 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, 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 @@ -234,14 +217,13 @@ class CarInterface(CarInterfaceBase): # 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 = 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 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 a7d2ec37bb..590840851d 100755 --- a/selfdrive/car/toyota/radar_interface.py +++ b/selfdrive/car/toyota/radar_interface.py @@ -4,6 +4,7 @@ from cereal import car 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 0852991611..ca58126793 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 c8f48c9768..99937561a7 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,12 +19,18 @@ 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" AVALON = "TOYOTA AVALON 2016" AVALON_2019 = "TOYOTA AVALON 2019" AVALONH_2019 = "TOYOTA AVALON HYBRID 2019" + AVALON_TSS2 = "TOYOTA AVALON 2022" CAMRY = "TOYOTA CAMRY 2018" CAMRYH = "TOYOTA CAMRY HYBRID 2018" CAMRY_TSS2 = "TOYOTA CAMRY 2021" # TSS 2.5 @@ -39,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" @@ -66,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): [ @@ -166,6 +173,23 @@ FW_VERSIONS = { b'8646F0702100\x00\x00\x00\x00', ], }, + CAR.AVALON_TSS2: { + (Ecu.esp, 0x7b0, None): [ + b'\x01F152607280\x00\x00\x00\x00\x00\x00', + ], + (Ecu.eps, 0x7a1, None): [ + b'8965B41110\x00\x00\x00\x00\x00\x00', + ], + (Ecu.engine, 0x700, None): [ + b'\x01896630742000\x00\x00\x00\x00', + ], + (Ecu.fwdRadar, 0x750, 0xf): [ + b'\x018821F6201200\x00\x00\x00\x00', + ], + (Ecu.fwdCamera, 0x750, 0x6d): [ + b'\x028646F4104100\x00\x00\x00\x008646G5301200\x00\x00\x00\x00', + ], + }, CAR.CAMRY: { (Ecu.engine, 0x700, None): [ b'\x018966306L3100\x00\x00\x00\x00', @@ -344,12 +368,14 @@ FW_VERSIONS = { 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', @@ -376,6 +402,7 @@ FW_VERSIONS = { }, 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', @@ -559,6 +586,7 @@ FW_VERSIONS = { 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', @@ -570,6 +598,7 @@ FW_VERSIONS = { 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', @@ -584,11 +613,13 @@ FW_VERSIONS = { 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', @@ -616,6 +647,7 @@ FW_VERSIONS = { b'\x01896637624000\x00\x00\x00\x00', b'\x01896637626000\x00\x00\x00\x00', b'\x01896637648000\x00\x00\x00\x00', + b'\x01896637643000\x00\x00\x00\x00', b'\x02896630ZJ5000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', b'\x02896630ZN8000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', b'\x02896630ZQ3000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', @@ -792,9 +824,11 @@ FW_VERSIONS = { 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', @@ -952,6 +986,23 @@ FW_VERSIONS = { 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', @@ -1504,20 +1555,21 @@ FW_VERSIONS = { }, 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', @@ -1529,8 +1581,9 @@ FW_VERSIONS = { 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: { @@ -1591,63 +1644,79 @@ FW_VERSIONS = { (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_nodsu_hybrid_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.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 824791ea94..b4fe01cd41 100644 --- a/selfdrive/car/volkswagen/carcontroller.py +++ b/selfdrive/car/volkswagen/carcontroller.py @@ -26,7 +26,7 @@ class CarController(): self.acc_starting = False self.acc_stopping = False - def update(self, enabled, CS, frame, ext_bus, actuators, visual_alert, left_lane_visible, right_lane_visible, left_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, lead_visible, set_speed, speed_visible): """ Controls thread """ @@ -98,7 +98,7 @@ class CarController(): # 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 diff --git a/selfdrive/car/volkswagen/carstate.py b/selfdrive/car/volkswagen/carstate.py index 86731474a2..898ab194e4 100644 --- a/selfdrive/car/volkswagen/carstate.py +++ b/selfdrive/car/volkswagen/carstate.py @@ -33,7 +33,7 @@ class CarState(CarStateBase): # 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 @@ -150,50 +150,49 @@ class CarState(CarStateBase): @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 = [ @@ -215,15 +214,15 @@ class CarState(CarStateBase): ] 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 @@ -237,18 +236,17 @@ class CarState(CarStateBase): @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 @@ -267,17 +265,17 @@ class CarState(CarStateBase): class MqbExtraSignals: # Additional signal and message lists for optional or bus-portable controllers fwd_radar_signals = [ - ("ACC_Wunschgeschw", "ACC_02", 0), # ACC set speed - ("ACC_Charisma_FahrPr", "ACC_04", 0), # Driving profile selection - ("ACC_Charisma_Status", "ACC_04", 0), # Driving profile status - ("ACC_Charisma_Umschaltung", "ACC_04", 0), # Driving profile switching - ("ACC_Texte_braking_guard", "ACC_04", 0), # Part of ACC driver alerts in instrument cluster - ("ACC_Typ", "ACC_06", 0), # Basic vs F2S vs SNG - ("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 - ("Unknown_Osc_1", "ACC_13", 0), # Unknown oscillating value (checksum/xor?) - ("Unknown_Osc_2", "ACC_13", 0), # Unknown oscillating value (checksum/xor?) + ("ACC_Wunschgeschw", "ACC_02"), # ACC set speed + ("ACC_Charisma_FahrPr", "ACC_04"), # Driving profile selection + ("ACC_Charisma_Status", "ACC_04"), # Driving profile status + ("ACC_Charisma_Umschaltung", "ACC_04"), # Driving profile switching + ("ACC_Texte_braking_guard", "ACC_04"), # Part of ACC driver alerts in instrument cluster + ("ACC_Typ", "ACC_06"), # Basic vs F2S vs SNG + ("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 + ("Unknown_Osc_1", "ACC_13"), # Unknown oscillating value (checksum/xor?) + ("Unknown_Osc_2", "ACC_13"), # Unknown oscillating value (checksum/xor?) ] fwd_radar_checks = [ ("ACC_06", 50), # From J428 ACC radar control module @@ -287,10 +285,10 @@ class MqbExtraSignals: ("ACC_13", 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 9ef41bd476..e6fdd77018 100644 --- a/selfdrive/car/volkswagen/interface.py +++ b/selfdrive/car/volkswagen/interface.py @@ -54,7 +54,7 @@ class CarInterface(CarInterfaceBase): # 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 @@ -194,12 +194,6 @@ class CarInterface(CarInterfaceBase): 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: @@ -251,7 +245,7 @@ class CarInterface(CarInterfaceBase): 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 6591babcfb..31575c29b1 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 @@ -129,6 +127,7 @@ FW_VERSIONS = { 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', @@ -139,17 +138,21 @@ FW_VERSIONS = { (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', @@ -167,6 +170,7 @@ FW_VERSIONS = { 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', @@ -213,6 +217,7 @@ FW_VERSIONS = { 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 9b496fef72..8dfeecbdd7 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 29dc17be90..e53d03f87b 100644 --- a/selfdrive/common/params.cc +++ b/selfdrive/common/params.cc @@ -88,12 +88,11 @@ std::unordered_map keys = { {"AthenadUploadQueue", PERSISTENT}, {"CalibrationParams", PERSISTENT}, {"CarBatteryCapacity", PERSISTENT}, - {"CarParams", CLEAR_ON_MANAGER_START | CLEAR_ON_PANDA_DISCONNECT | CLEAR_ON_IGNITION_ON}, - {"CarParamsCache", CLEAR_ON_MANAGER_START | CLEAR_ON_PANDA_DISCONNECT}, - {"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}, @@ -131,8 +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}, @@ -141,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}, {"PrimeRedirected", PERSISTENT}, {"RecordFront", PERSISTENT}, @@ -162,7 +164,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 c4bdde0012..be09c63d54 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 0000000000..27dfc2ca9d --- /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 0000000000..5d223bb666 --- /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 74488e2201..1fe700415d 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 9a1d3c0a67..6403820ac8 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 2b7a3c6eb8..1350b3b825 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 0000000000..1d00def63d --- /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 9a6a4d9bdb..bf0df3bcaa 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 0469d50be2..f9260762ed 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -31,15 +31,13 @@ from selfdrive.manager.process_config import managed_processes 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", - "statsd"} | \ + "statsd", "shutdownd"} | \ {k for k, v in managed_processes.items() if not v.enabled} ACTUATOR_FIELDS = set(car.CarControl.Actuators.schema.fields.keys()) @@ -55,7 +53,7 @@ ButtonEvent = car.CarState.ButtonEvent SafetyModel = car.CarParams.SafetyModel IGNORED_SAFETY_MODES = [SafetyModel.silent, SafetyModel.noOutput] - +CSID_MAP = {"0": EventName.roadCameraError, "1": EventName.wideRoadCameraError, "2": EventName.driverCameraError} class Controls: def __init__(self, sm=None, pm=None, can_sock=None): @@ -100,7 +98,6 @@ class Controls: # 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 @@ -110,11 +107,7 @@ class Controls: 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 @@ -133,13 +126,13 @@ class Controls: 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) self.initialized = False self.state = State.disabled @@ -153,7 +146,6 @@ class Controls: 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 = [] @@ -169,8 +161,6 @@ class Controls: 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: @@ -191,10 +181,8 @@ class Controls: """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 @@ -204,6 +192,9 @@ class Controls: 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: @@ -260,9 +251,12 @@ class Controls: 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) @@ -279,7 +273,7 @@ class Controls: 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 @@ -311,25 +305,17 @@ class Controls: 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: @@ -373,6 +359,10 @@ class Controls: 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 @@ -390,7 +380,7 @@ class Controls: 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 @@ -440,7 +430,7 @@ class Controls: # 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: @@ -526,19 +516,8 @@ class Controls: 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 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 @@ -563,7 +542,7 @@ class Controls: 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 @@ -601,7 +580,7 @@ class Controls: ldw_allowed = self.is_ldw_enabled and CS.vEgo > LDW_MIN_SPEED and not recent_blinker \ and not self.active and self.sm['liveCalibration'].calStatus == Calibration.CALIBRATED - model_v2 = self.sm['modelV2'] + model_v2 = self.sm['modelV2'] desire_prediction = model_v2.meta.desirePrediction if len(desire_prediction) and ldw_allowed: right_lane_visible = self.sm['lateralPlan'].rProb > 0.5 @@ -619,10 +598,15 @@ class Controls: 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 bf93b5f47e..2dad05e214 100644 --- a/selfdrive/controls/lib/alertmanager.py +++ b/selfdrive/controls/lib/alertmanager.py @@ -40,20 +40,20 @@ class AlertManager: 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 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 4affe0cfe3..aa54f582f6 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 0000000000..c34d143a5a --- /dev/null +++ b/selfdrive/controls/lib/desire_helper.py @@ -0,0 +1,113 @@ +from cereal import log +from common.realtime import DT_MDL +from selfdrive.config import Conversions as CV + +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 + + 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)) + + 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 cd139c062a..514d2f0cc2 100644 --- a/selfdrive/controls/lib/events.py +++ b/selfdrive/controls/lib/events.py @@ -93,7 +93,7 @@ class Events: 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 @@ -264,8 +264,6 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = { EventName.stockFcw: {}, - EventName.lkasDisabled: {}, - # ********** events only containing alerts displayed in all states ********** EventName.joystickDebug: { @@ -316,14 +314,6 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = { #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 @@ -831,4 +821,9 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = { 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/latcontrol.py b/selfdrive/controls/lib/latcontrol.py new file mode 100644 index 0000000000..eb16aca2e8 --- /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 dc184cf58b..c935356311 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 @@ class LatControlAngle(): 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 75b27ac8c1..dc1b31bad9 100644 --- a/selfdrive/controls/lib/latcontrol_indi.py +++ b/selfdrive/controls/lib/latcontrol_indi.py @@ -5,13 +5,13 @@ from cereal import log 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 @@ class LatControlINDI(): 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 @@ class LatControlINDI(): 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 @@ class LatControlINDI(): 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 @@ class LatControlINDI(): 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 @@ class LatControlINDI(): 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 5777cde8e8..5c273a45be 100644 --- a/selfdrive/controls/lib/latcontrol_lqr.py +++ b/selfdrive/controls/lib/latcontrol_lqr.py @@ -5,10 +5,12 @@ from common.numpy_fast import clip 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 @@ class LatControlLQR(): 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 @@ class LatControlLQR(): 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 @@ class LatControlLQR(): 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_pid.py b/selfdrive/controls/lib/latcontrol_pid.py index bcb8ccba56..f5ff5a95e5 100644 --- a/selfdrive/controls/lib/latcontrol_pid.py +++ b/selfdrive/controls/lib/latcontrol_pid.py @@ -2,18 +2,20 @@ import math from selfdrive.controls.lib.pid import PIController 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 = PIController((CP.lateralTuning.pid.kpBP, CP.lateralTuning.pid.kpV), (CP.lateralTuning.pid.kiBP, CP.lateralTuning.pid.kiV), - k_f=CP.lateralTuning.pid.kf, pos_limit=1.0, neg_limit=-1.0, - sat_limit=CP.steerLimitTimer) + k_f=CP.lateralTuning.pid.kf, pos_limit=1.0, neg_limit=-1.0) 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): @@ -26,7 +28,7 @@ class LatControlPID(): 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() @@ -40,14 +42,13 @@ class LatControlPID(): 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 6162cf68a5..eeda25627a 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 os 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 @@ else: 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 @@ class LateralMpc(): # 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 @@ class LateralMpc(): #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 1d7684e827..a9c6411394 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,54 +5,20 @@ from selfdrive.swaglog import cloudlog 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 -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 LateralPlanner: def __init__(self, CP, use_lanelines=True, wide_camera=False): self.use_lanelines = use_lanelines self.LP = LanePlanner(wide_camera) + 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)) @@ -62,19 +27,19 @@ class LateralPlanner: 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) @@ -83,84 +48,15 @@ class LateralPlanner: 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)) - - 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) @@ -170,23 +66,24 @@ class LateralPlanner: # 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() @@ -216,10 +113,11 @@ class LateralPlanner: lateralPlan.dProb = float(self.LP.d_prob) 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 b14ffdc6c8..21c34aa2d6 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -44,8 +44,7 @@ class LongControl(): self.long_control_state = LongCtrlState.off # initialized to off self.pid = PIController((CP.longitudinalTuning.kpBP, CP.longitudinalTuning.kpV), (CP.longitudinalTuning.kiBP, CP.longitudinalTuning.kiV), - rate=1/DT_CTRL, - sat_limit=0.8) + rate=1 / DT_CTRL) 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 c07110eefa..8625df745a 100644 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -215,6 +215,7 @@ class LongitudinalMpc: 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() @@ -356,7 +357,11 @@ class LongitudinalMpc: 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): @@ -368,7 +373,6 @@ class LongitudinalMpc: 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 9dc6ab9ee5..8c79404058 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -120,4 +120,6 @@ class Planner: 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 e384aaaedf..28c6438191 100644 --- a/selfdrive/controls/lib/pid.py +++ b/selfdrive/controls/lib/pid.py @@ -13,7 +13,7 @@ def apply_deadzone(error, deadzone): return error class PIController(): - def __init__(self, k_p, k_i, k_f=1., pos_limit=None, neg_limit=None, rate=100, sat_limit=0.8): + def __init__(self, k_p, k_i, k_f=1., pos_limit=None, neg_limit=None, rate=100): self._k_p = k_p # proportional gain self._k_i = k_i # integral gain self.k_f = k_f # feedforward gain @@ -25,10 +25,8 @@ class PIController(): 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.reset() @@ -40,27 +38,13 @@ class PIController(): def k_i(self): return interp(self.speed, self._k_i[0], self._k_i[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 - 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)) @@ -81,7 +65,6 @@ class PIController(): self.i = i control = self.p + self.f + self.i - self.saturated = self._check_saturation(control, check_saturation, error) self.control = clip(control, self.neg_limit, self.pos_limit) return self.control diff --git a/selfdrive/controls/lib/tests/test_alertmanager.py b/selfdrive/controls/lib/tests/test_alertmanager.py index 2b606390c7..6c1b6fc4a2 100755 --- a/selfdrive/controls/lib/tests/test_alertmanager.py +++ b/selfdrive/controls/lib/tests/test_alertmanager.py @@ -34,7 +34,7 @@ class TestAlertManager(unittest.TestCase): 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 0000000000..8345840eca --- /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 d4b0733692..65f8480c7c 100755 --- a/selfdrive/controls/radard.py +++ b/selfdrive/controls/radard.py @@ -172,7 +172,7 @@ class RadarD(): 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_lateral_mpc.py b/selfdrive/controls/tests/test_lateral_mpc.py index 7c4fe7b6ad..4630e28a61 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 658adf499e..9d13453045 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 @@ class TestStartup(unittest.TestCase): 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 e42b7532b0..0000000000 --- a/selfdrive/crash.py +++ /dev/null @@ -1,27 +0,0 @@ -"""Install exception handler for process crash.""" -from selfdrive.swaglog import cloudlog -from selfdrive.version import get_version - -import sentry_sdk -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 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_sdk.init("https://a8dc76b5bfb34908a601d67e2aa8bcf9@o33823.ingest.sentry.io/77924", - default_integrations=False, integrations=[ThreadingIntegration(propagate_hub=True)], - release=get_version()) diff --git a/selfdrive/debug/adb.sh b/selfdrive/debug/adb.sh index 82a3ddbb94..8a04d4aefd 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 b4436bd2be..0aaaf1350b 100755 --- a/selfdrive/debug/can_printer.py +++ b/selfdrive/debug/can_printer.py @@ -7,7 +7,7 @@ import cereal.messaging as messaging 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 @@ if __name__ == "__main__": 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/profiling/snapdragon/.gitignore b/selfdrive/debug/profiling/snapdragon/.gitignore new file mode 100644 index 0000000000..5d3033efb3 --- /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 0000000000..ee826b413a --- /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 0000000000..5099fb09cb --- /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 b836eb0129..3d91468b90 100755 --- a/selfdrive/hardware/eon/androidd.py +++ b/selfdrive/hardware/eon/androidd.py @@ -4,13 +4,14 @@ import time 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,18 @@ 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"): + 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 +80,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/neos.json b/selfdrive/hardware/eon/neos.json index 228029e9fb..4010f7126a 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 0000000000..bc17af00dd --- /dev/null +++ b/selfdrive/hardware/eon/shutdownd.py @@ -0,0 +1,26 @@ +#!/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()}") + + 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 d7a6c8d1d5..f18ede01ec 100644 --- a/selfdrive/hardware/hw.h +++ b/selfdrive/hardware/hw.h @@ -20,17 +20,16 @@ public: #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/locationd/calibrationd.py b/selfdrive/locationd/calibrationd.py index f2a534d370..ae191bbebb 100755 --- a/selfdrive/locationd/calibrationd.py +++ b/selfdrive/locationd/calibrationd.py @@ -79,12 +79,12 @@ class Calibrator(): def reset(self, rpy_init=RPY_INIT, valid_blocks=0, smooth_from=None): if not np.isfinite(rpy_init).all(): - self.rpy = RPY_INIT.copy() + self.rpy = RPY_INIT.copy() else: 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 @@ -108,9 +108,12 @@ class Calibrator(): 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) @@ -165,8 +168,6 @@ class Calibrator(): 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() diff --git a/selfdrive/locationd/locationd.cc b/selfdrive/locationd/locationd.cc index 35fbd90e56..b723878e9d 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 03de2d6462..9557568ba2 100755 --- a/selfdrive/locationd/paramsd.py +++ b/selfdrive/locationd/paramsd.py @@ -45,10 +45,12 @@ class ParamsLearner: 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/ublox_msg.h b/selfdrive/locationd/ublox_msg.h index b8a48ad2da..da1c180c0b 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 4452e2f093..1a982a0feb 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 5b908d3e87..a98ae5834d 100755 --- a/selfdrive/logcatd/tests/test_logcatd_android.py +++ b/selfdrive/logcatd/tests/test_logcatd_android.py @@ -34,7 +34,11 @@ class TestLogcatdAndroid(unittest.TestCase): 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 478eb16852..c5897091dc 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/loggerd.cc b/selfdrive/loggerd/loggerd.cc index 37f03ef4e5..d402c5787f 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 bdf5ef8f9d..caacbc591f 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 b0f9f7a9c8..f103822a13 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 9cef7ddcab..75d906784d 100644 --- a/selfdrive/loggerd/raw_logger.h +++ b/selfdrive/loggerd/raw_logger.h @@ -39,4 +39,5 @@ private: 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 20cd61bad5..13e0b720d8 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 cereal.services import service_list 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 @@ CEREAL_SERVICES = [f for f in log.Event.schema.union_fields if f in service_list 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 @@ class TestLoggerd(unittest.TestCase): 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 aa97f0c777..e721692500 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 76106e473f..21b8cb9657 100755 --- a/selfdrive/manager/manager.py +++ b/selfdrive/manager/manager.py @@ -8,7 +8,7 @@ import traceback 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.manager.process_config import managed_processes 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_comma_remote + terms_version, training_version sys.path.append(os.path.join(BASEDIR, "pyextra")) @@ -90,15 +90,11 @@ 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()) - if is_comma_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(), 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(): @@ -124,8 +120,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") @@ -141,9 +137,6 @@ def manager_thread() -> None: sm.update() not_run = ignore[:] - if sm['deviceState'].freeSpacePercent < 5: - not_run.append("loggerd") - started = sm['deviceState'].started driverview = params.get_bool("IsDriverViewEnabled") ensure_running(managed_processes.values(), started, driverview, not_run) @@ -169,8 +162,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 @@ -197,7 +191,7 @@ def main() -> None: manager_thread() except Exception: traceback.print_exc() - crash.capture_exception() + sentry.capture_exception() finally: manager_cleanup() @@ -222,6 +216,11 @@ if __name__ == "__main__": 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 2dcb42f339..ebdd2a90bf 100644 --- a/selfdrive/manager/process.py +++ b/selfdrive/manager/process.py @@ -11,7 +11,7 @@ from multiprocessing import Process 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) @@ -203,7 +206,7 @@ class NativeProcess(ManagerProcess): 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 diff --git a/selfdrive/manager/process_config.py b/selfdrive/manager/process_config.py index b5fc01364e..c017e48fab 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 WEBCAM = os.getenv("USE_WEBCAM") is not None @@ -40,6 +40,7 @@ procs = [ # 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 1728cc9e79..f1b0c42bd9 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/pandad.py b/selfdrive/pandad.py index e3ebdad637..1f9fddab7a 100755 --- a/selfdrive/pandad.py +++ b/selfdrive/pandad.py @@ -13,7 +13,7 @@ from common.params import Params 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 0000000000..5f22bf18e0 --- /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 index 33f35032ff..2e62e32536 100755 --- a/selfdrive/statsd.py +++ b/selfdrive/statsd.py @@ -4,6 +4,8 @@ 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 @@ -20,14 +22,14 @@ class StatLog: def __init__(self): self.pid = None - def connect(self): + 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): + def _send(self, metric: str) -> None: if os.getpid() != self.pid: self.connect() @@ -37,16 +39,17 @@ class StatLog: # drop :/ pass - def gauge(self, name: str, value: float): + def gauge(self, name: str, value: float) -> None: self._send(f"{name}:{value}|{METRIC_TYPE.GAUGE}") -def main(): - def get_influxdb_line(measurement: str, value: float, timestamp: datetime, tags: dict): +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 tag_key in tags.keys(): - res += f",{tag_key}={str(tags[tag_key])}" - res += f" value={value} {int(timestamp.timestamp() * 1e9)}\n" + 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 @@ -59,7 +62,6 @@ def main(): # initialize tags tags = { - 'dongleId': Params().get("DongleId", encoding='utf-8'), 'started': False, 'version': get_short_version(), 'branch': get_short_branch(), @@ -101,7 +103,7 @@ def main(): current_time = datetime.utcnow().replace(tzinfo=timezone.utc) tags['started'] = sm['deviceState'].started - for gauge_key in gauges.keys(): + for gauge_key in gauges: result += get_influxdb_line(f"gauge.{gauge_key}", gauges[gauge_key], current_time, tags) # clear intermediate data diff --git a/selfdrive/test/helpers.py b/selfdrive/test/helpers.py index 0bb475c929..5abc0d964f 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 35b9f32e68..60aaeb724d 100755 --- a/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py +++ b/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py @@ -120,7 +120,6 @@ class LongitudinalControl(unittest.TestCase): 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 72f5e51bb2..de734e52fb 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 8a13e4b18e..da9c717285 100755 --- a/selfdrive/test/process_replay/process_replay.py +++ b/selfdrive/test/process_replay/process_replay.py @@ -25,7 +25,7 @@ NUMPY_TOLERANCE = 1e-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 @@ class DumbSocket: 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 @@ CONFIGS = [ 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 @@ CONFIGS = [ "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 6ebdb327b3..9b81f7aeaa 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 5326075240..16ee5e9a28 100755 --- a/selfdrive/test/process_replay/regen.py +++ b/selfdrive/test/process_replay/regen.py @@ -11,8 +11,8 @@ import cereal.messaging as messaging 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 @@ process_replay_dir = os.path.dirname(os.path.abspath(__file__)) 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_cameras(lr, frs): 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 047c83f465..4f057bbf50 --- a/selfdrive/test/process_replay/regen_all.py +++ b/selfdrive/test/process_replay/regen_all.py @@ -7,12 +7,13 @@ if __name__ == "__main__": 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 6c33b46224..f7dae5c9fb 100755 --- a/selfdrive/test/process_replay/test_processes.py +++ b/selfdrive/test/process_replay/test_processes.py @@ -30,18 +30,18 @@ original_segments = [ ] 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 4ea60a76a8..01c7d753c3 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 5112d30919..f8f6da6d7a 100755 --- a/selfdrive/test/test_models.py +++ b/selfdrive/test/test_models.py @@ -4,16 +4,19 @@ import os 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 @@ PandaType = log.PandaState.PandaType 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 @@ class TestCarModel(unittest.TestCase): 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) @@ -179,53 +185,57 @@ class TestCarModel(unittest.TestCase): """ 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 2f85605b83..1136bc6b26 100755 --- a/selfdrive/test/test_onroad.py +++ b/selfdrive/test/test_onroad.py @@ -34,7 +34,7 @@ PROCS = { "./_modeld": 4.48, "./boardd": 3.63, "./_dmonitoringmodeld": 2.67, - "selfdrive.thermald.thermald": 7.73, + "selfdrive.thermald.thermald": 5.36, "selfdrive.locationd.calibrationd": 2.0, "./_soundd": 1.0, "selfdrive.monitoring.dmonitoringd": 1.90, @@ -61,7 +61,7 @@ if TICI: "./_dmonitoringmodeld": 10.0, "selfdrive.locationd.paramsd": 5.0, "selfdrive.controls.radard": 3.6, - "selfdrive.thermald.thermald": 4.75, + "selfdrive.thermald.thermald": 3.87, }) @@ -204,6 +204,22 @@ class TestOnroad(unittest.TestCase): 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 f064b89f51..02bcd36b19 --- 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 @@ routes = [ 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 @@ routes = [ 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), @@ -107,6 +105,7 @@ routes = [ TestRoute("000cf3730200c71c|2021-05-24--10-42-05", TOYOTA.AVALON), TestRoute("0bb588106852abb7|2021-05-26--12-22-01", TOYOTA.AVALON_2019), TestRoute("87bef2930af86592|2021-05-30--09-40-54", TOYOTA.AVALONH_2019), + TestRoute("e9966711cfb04ce3|2022-01-11--07-59-43", TOYOTA.AVALON_TSS2), TestRoute("6cdecc4728d4af37|2020-02-23--15-44-18", TOYOTA.CAMRY), TestRoute("3456ad0cd7281b24|2020-12-13--17-45-56", TOYOTA.CAMRY_TSS2), TestRoute("ffccc77938ddbc44|2021-01-04--16-55-41", TOYOTA.CAMRYH_TSS2), @@ -115,8 +114,6 @@ routes = [ 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), @@ -144,6 +141,7 @@ routes = [ 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), @@ -193,23 +191,8 @@ routes = [ TestRoute("74f1038827005090|2020-08-26--20-05-50", MAZDA.MAZDA3), TestRoute("fb53c640f499b73d|2021-06-01--04-17-56", MAZDA.MAZDA6), TestRoute("f6d5b1a9d7a1c92e|2021-07-08--06-56-59", MAZDA.CX9_2021), + TestRoute("a4af1602d8e668ac|2022-02-03--12-17-07", MAZDA.CX5_2022), 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/thermald.py b/selfdrive/thermald/thermald.py index 7982afff23..6cbb8ee89d 100755 --- a/selfdrive/thermald/thermald.py +++ b/selfdrive/thermald/thermald.py @@ -1,29 +1,31 @@ #!/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 PIController -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 -from selfdrive.statsd import statlog ThermalStatus = log.DeviceState.ThermalStatus NetworkType = log.DeviceState.NetworkType @@ -31,8 +33,10 @@ NetworkStrength = log.DeviceState.NetworkStrength 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. @@ -153,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() - 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", "controlsState"]) + 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(), + ) + + 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"], poll=["pandaStates"]) fan_speed = 0 count = 0 @@ -176,27 +217,24 @@ 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() @@ -204,26 +242,18 @@ def thermald_thread() -> NoReturn: # TODO: use PI controller for UNO controller = PIController(k_p=0, k_i=2e-3, 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 @@ -244,57 +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 - for i, temp in enumerate(nvme_temps): - statlog.gauge(f"nvme_temperature{i}", temp) - if modem_temps is not None: - msg.deviceState.modemTempC = modem_temps - for i, temp in enumerate(modem_temps): - statlog.gauge(f"modem_temperature{i}", temp) + + 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() @@ -346,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()) @@ -369,7 +366,7 @@ def thermald_thread() -> NoReturn: try: with open('/dev/kmsg', 'w') as kmsg: - kmsg.write(f"[thermald] engaged: {engaged}") + kmsg.write(f"<3>[thermald] engaged: {engaged}\n") except Exception: pass @@ -398,16 +395,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 @@ -426,7 +415,7 @@ def thermald_thread() -> NoReturn: should_start_prev = should_start startup_conditions_prev = startup_conditions.copy() - # log more stats + # 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) @@ -440,6 +429,10 @@ def thermald_thread() -> NoReturn: 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) @@ -450,7 +443,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())) @@ -458,8 +451,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 df96c222c0..459ab2f0f9 100755 --- a/selfdrive/tombstoned.py +++ b/selfdrive/tombstoned.py @@ -9,14 +9,12 @@ import time 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 62f93c13a9..ed057bf54b 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 2d5651dcce..9c2088b1d7 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 037ef82f67..e78d65ef0f 100644 --- a/selfdrive/ui/qt/offroad/networking.h +++ b/selfdrive/ui/qt/offroad/networking.h @@ -65,6 +65,7 @@ private: 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 83802a1d90..73dbf0a845 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 0970173929..32ff0918ef 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 04defd32dd..49c107f555 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 e394d1248a..50e2e4de4b 100644 --- a/selfdrive/ui/qt/onroad.cc +++ b/selfdrive/ui/qt/onroad.cc @@ -338,7 +338,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 aa000f9359..9ffcf7f6cf 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 bd494327cc..17304665c7 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 0fee52f736..76d5a39d44 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 1912c9b581..9bcc00583e 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 0000000000..72e2fcf802 --- /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 0000000000..bce1146d46 --- /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 1585bb42d2..b57a62d501 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 59e43cb58f..ef90c28355 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 d836ef11f8..579fe50644 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 180aecc7e1..a7d2f54042 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 98e211fb53..fd1a4b1990 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 4f97990506..0fc171ba2d 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 @@ public: ~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: 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: 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: 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 fe6e21a91a..e6a6177149 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 c39eef7d92..ac8fba75ca 100644 --- a/selfdrive/ui/replay/route.h +++ b/selfdrive/ui/replay/route.h @@ -27,6 +27,7 @@ public: 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 6063dfe7d5..3cafa9b534 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 d6eba2e2a8..513cd965be 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 cd4b179cdc..6c808095e8 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 c33b61c19e..d3f33294a1 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -109,12 +109,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; @@ -131,6 +125,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) { @@ -164,16 +166,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()); @@ -218,6 +226,7 @@ UIState::UIState(QObject *parent) : QObject(parent) { sm = std::make_unique>({ "modelV2", "controlsState", "liveCalibration", "radarState", "deviceState", "roadCameraState", "pandaStates", "carParams", "driverMonitoringState", "sensorEvents", "carState", "liveLocationKalman", + "wideRoadCameraState", }); Params params; diff --git a/selfdrive/version.py b/selfdrive/version.py index 0c1a44f236..f2570cd305 100644 --- a/selfdrive/version.py +++ b/selfdrive/version.py @@ -79,6 +79,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 @@ -109,10 +111,6 @@ def is_dirty() -> bool: pass dirty = (subprocess.call(["git", "diff-index", "--quiet", branch, "--"]) != 0) - - dirty = dirty or (not is_comma_remote()) - dirty = dirty or ('master' in branch) - except subprocess.CalledProcessError: cloudlog.exception("git subprocess failed while checking dirty") dirty = True diff --git a/tools/joystick/joystickd.py b/tools/joystick/joystickd.py index 2b138b9c17..5a86293441 100755 --- a/tools/joystick/joystickd.py +++ b/tools/joystick/joystickd.py @@ -73,7 +73,7 @@ def joystick_thread(use_keyboard): if __name__ == '__main__': - parser = argparse.ArgumentParser(description='Publishes events from your joystick to control your car.\n' + 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') @@ -85,11 +85,11 @@ if __name__ == '__main__': print() if args.keyboard: - print('Gas/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('Using joystick, make sure to run cereal/messaging/bridge on your device if running over the network!') diff --git a/tools/lib/bootlog.py b/tools/lib/bootlog.py new file mode 100644 index 0000000000..dc1cf35c52 --- /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 3d4c46b220..4ea9ad4290 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 ac5962462e..bdf48bb438 100644 --- a/tools/lib/framereader.py +++ b/tools/lib/framereader.py @@ -17,10 +17,7 @@ from tools.lib.cache import cache_path_for_file_path 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 0000000000..efe704b9ec --- /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 fc7a8dcb70..c8d506b4b3 100755 --- a/tools/lib/logreader.py +++ b/tools/lib/logreader.py @@ -5,10 +5,7 @@ import bz2 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 c87bd04b9e..8092096a0b 100644 --- a/tools/lib/route.py +++ b/tools/lib/route.py @@ -6,11 +6,7 @@ from itertools import chain 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 @@ class Route: 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 7cafaf60ac..1dcbcae0c9 100755 --- a/tools/plotjuggler/juggle.py +++ b/tools/plotjuggler/juggle.py @@ -23,6 +23,7 @@ juggle_dir = os.path.dirname(os.path.realpath(__file__)) 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 @@ if __name__ == "__main__": 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 0000000000..e50b5ffb34 --- /dev/null +++ b/tools/plotjuggler/layouts/controls_mismatch_debug.xml @@ -0,0 +1,46 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/tools/sim/lib/can.py b/tools/sim/lib/can.py index 4873947d4b..af0f415bbc 100755 --- a/tools/sim/lib/can.py +++ b/tools/sim/lib/can.py @@ -12,11 +12,11 @@ rpacker = CANPacker("acura_ilx_2016_nidec") 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),