diff --git a/.github/workflows/prebuilt.yaml b/.github/workflows/prebuilt.yaml index c8b4c51e38..1fd093e2ac 100644 --- a/.github/workflows/prebuilt.yaml +++ b/.github/workflows/prebuilt.yaml @@ -16,7 +16,6 @@ jobs: build_prebuilt: name: build prebuilt runs-on: ubuntu-20.04 - timeout-minutes: 60 if: github.repository == 'commaai/openpilot' env: IMAGE_NAME: openpilot-prebuilt diff --git a/.github/workflows/release.yaml b/.github/workflows/release.yaml index 8df89dcc38..7f83f61e0a 100644 --- a/.github/workflows/release.yaml +++ b/.github/workflows/release.yaml @@ -8,7 +8,6 @@ jobs: build_masterci: name: build master-ci runs-on: ubuntu-20.04 - timeout-minutes: 60 if: github.repository == 'commaai/openpilot' steps: - name: Wait for green check mark diff --git a/.github/workflows/selfdrive_tests.yaml b/.github/workflows/selfdrive_tests.yaml index 7cc45d23b9..834b2141d0 100644 --- a/.github/workflows/selfdrive_tests.yaml +++ b/.github/workflows/selfdrive_tests.yaml @@ -32,7 +32,6 @@ jobs: build_release: name: build release runs-on: ubuntu-20.04 - timeout-minutes: 30 env: STRIPPED_DIR: /tmp/releasepilot steps: @@ -61,12 +60,11 @@ jobs: cp .pylintrc $STRIPPED_DIR cp mypy.ini $STRIPPED_DIR cd $STRIPPED_DIR - ${{ env.RUN }} "SKIP=test_translations pre-commit run --all" + ${{ env.RUN }} "pre-commit run --all" build_all: name: build all runs-on: ubuntu-20.04 - timeout-minutes: 30 steps: - uses: actions/checkout@v3 with: @@ -84,7 +82,6 @@ jobs: #build_mac: # name: build macos # runs-on: macos-latest - # timeout-minutes: 60 # steps: # - uses: actions/checkout@v3 # with: @@ -141,7 +138,6 @@ jobs: docker_push: name: docker push runs-on: ubuntu-20.04 - timeout-minutes: 22 if: github.ref == 'refs/heads/master' && github.event_name != 'pull_request' && github.repository == 'commaai/openpilot' needs: static_analysis # hack to ensure slow tests run first since this and static_analysis are fast steps: @@ -150,14 +146,12 @@ jobs: submodules: true - name: Build Docker image run: eval "$BUILD" - timeout-minutes: 13 - name: Push to container registry run: | $DOCKER_LOGIN docker push $DOCKER_REGISTRY/$BASE_IMAGE:latest - name: Build CL Docker image run: eval "$BUILD_CL" - timeout-minutes: 4 - name: Push to container registry run: | $DOCKER_LOGIN @@ -166,7 +160,6 @@ jobs: static_analysis: name: static analysis runs-on: ubuntu-20.04 - timeout-minutes: 20 steps: - uses: actions/checkout@v3 with: @@ -174,13 +167,11 @@ jobs: - name: Build Docker image run: eval "$BUILD" - name: pre-commit - timeout-minutes: 5 run: ${{ env.RUN }} "pre-commit run --all" valgrind: name: valgrind runs-on: ubuntu-20.04 - timeout-minutes: 20 steps: - uses: actions/checkout@v3 with: @@ -198,7 +189,6 @@ jobs: unit_tests: name: unit tests runs-on: ubuntu-20.04 - timeout-minutes: 30 steps: - uses: actions/checkout@v3 with: @@ -207,7 +197,6 @@ jobs: - name: Build openpilot run: ${{ env.RUN }} "scons -j$(nproc)" - name: Run unit tests - timeout-minutes: 15 run: | ${{ env.RUN }} "export SKIP_LONG_TESTS=1 && \ $UNIT_TEST common && \ @@ -219,6 +208,7 @@ jobs: $UNIT_TEST selfdrive/car && \ $UNIT_TEST selfdrive/locationd && \ selfdrive/locationd/test/_test_locationd_lib.py && \ + ./selfdrive/locationd/test/test_glonass_runner && \ $UNIT_TEST selfdrive/athena && \ $UNIT_TEST selfdrive/thermald && \ $UNIT_TEST system/hardware/tici && \ @@ -241,7 +231,6 @@ jobs: process_replay: name: process replay runs-on: ubuntu-20.04 - timeout-minutes: 25 steps: - uses: actions/checkout@v3 with: @@ -279,7 +268,6 @@ jobs: test_modeld: name: model tests runs-on: ubuntu-20.04 - timeout-minutes: 20 steps: - uses: actions/checkout@v3 with: @@ -292,12 +280,10 @@ jobs: run: | ${{ env.RUN }} "scons -j$(nproc)" - name: Run model replay with ONNX - timeout-minutes: 2 run: | ${{ env.RUN_CL }} "ONNXCPU=1 CI=1 NO_NAV=1 coverage run selfdrive/test/process_replay/model_replay.py && \ coverage xml" - name: Run unit tests - timeout-minutes: 5 run: | ${{ env.RUN_CL }} "$UNIT_TEST selfdrive/modeld && \ coverage xml" @@ -307,7 +293,6 @@ jobs: test_longitudinal: name: longitudinal runs-on: ubuntu-20.04 - timeout-minutes: 20 steps: - uses: actions/checkout@v3 with: @@ -322,7 +307,6 @@ jobs: cd selfdrive/test/longitudinal_maneuvers && \ coverage run ./test_longitudinal.py && \ coverage xml" - timeout-minutes: 2 - name: "Upload coverage to Codecov" uses: codecov/codecov-action@v2 - uses: actions/upload-artifact@v2 @@ -335,7 +319,6 @@ jobs: test_cars: name: cars runs-on: ubuntu-20.04 - timeout-minutes: 20 strategy: fail-fast: false matrix: @@ -354,7 +337,6 @@ jobs: - name: Build openpilot run: ${{ env.RUN }} "scons -j$(nproc)" - name: Test car models - timeout-minutes: 12 run: | ${{ env.RUN }} "coverage run -m pytest selfdrive/car/tests/test_models.py && \ coverage xml && \ @@ -368,7 +350,6 @@ jobs: car_docs_diff: name: PR comments runs-on: ubuntu-20.04 - timeout-minutes: 20 if: github.event_name == 'pull_request' steps: - uses: actions/checkout@v3 diff --git a/.github/workflows/tools_tests.yaml b/.github/workflows/tools_tests.yaml index 94cc3c2580..3b32d3c790 100644 --- a/.github/workflows/tools_tests.yaml +++ b/.github/workflows/tools_tests.yaml @@ -30,7 +30,6 @@ jobs: plotjuggler: name: plotjuggler runs-on: ubuntu-20.04 - timeout-minutes: 20 steps: - uses: actions/checkout@v3 with: @@ -38,7 +37,6 @@ jobs: - name: Build Docker image run: eval "$BUILD" - name: Unit test - timeout-minutes: 2 run: | ${{ env.RUN }} "scons -j$(nproc) --directory=/tmp/openpilot/cereal && \ apt-get update && \ @@ -49,7 +47,6 @@ jobs: simulator: name: simulator runs-on: ubuntu-20.04 - timeout-minutes: 30 env: IMAGE_NAME: openpilot-sim if: github.repository == 'commaai/openpilot' @@ -74,7 +71,6 @@ jobs: docs: name: build docs runs-on: ubuntu-20.04 - timeout-minutes: 25 steps: - uses: actions/checkout@v3 with: diff --git a/Jenkinsfile b/Jenkinsfile index 3b16b3f112..30c045e419 100644 --- a/Jenkinsfile +++ b/Jenkinsfile @@ -13,6 +13,8 @@ export GIT_COMMIT=${env.GIT_COMMIT} export AZURE_TOKEN='${env.AZURE_TOKEN}' export MAPBOX_TOKEN='${env.MAPBOX_TOKEN}' +export GIT_SSH_COMMAND="ssh -i /data/gitkey" + source ~/.bash_profile if [ -f /TICI ]; then source /etc/profile @@ -56,14 +58,26 @@ pipeline { } stages { - stage('build release3') { + stage('build release3-staging') { agent { docker { image 'ghcr.io/commaai/alpine-ssh'; args '--user=root' } } when { branch 'devel-staging' } steps { phone_steps("tici-needs-can", [ - ["build release3-staging & dashcam3-staging", "PUSH=1 $SOURCE_DIR/release/build_release.sh"], + ["build release3-staging & dashcam3-staging", "RELEASE_BRANCH=release3-staging DASHCAM_BRANCH=dashcam3-staging $SOURCE_DIR/release/build_release.sh"], + ]) + } + } + + stage('build nightly') { + agent { docker { image 'ghcr.io/commaai/alpine-ssh'; args '--user=root' } } + when { + branch 'master-ci' + } + steps { + phone_steps("tici-needs-can", [ + ["build nightly", "RELEASE_BRANCH=nightly $SOURCE_DIR/release/build_release.sh"], ]) } } @@ -81,6 +95,7 @@ pipeline { parallel { + /* stage('simulator') { agent { dockerfile { @@ -108,6 +123,7 @@ pipeline { } } } + */ stage('build') { agent { docker { image 'ghcr.io/commaai/alpine-ssh'; args '--user=root' } } @@ -152,7 +168,7 @@ pipeline { stage('camerad-ar') { agent { docker { image 'ghcr.io/commaai/alpine-ssh'; args '--user=root' } } steps { - phone_steps("tici-ar0321", [ + phone_steps("tici-ar0231", [ ["build", "cd selfdrive/manager && ./build.py"], ["test camerad", "python system/camerad/test/test_camerad.py"], ["test exposure", "python system/camerad/test/test_exposure.py"], diff --git a/RELEASES.md b/RELEASES.md index 1b5ef8f7a7..f3d4cfb64f 100644 --- a/RELEASES.md +++ b/RELEASES.md @@ -1,13 +1,22 @@ -Version 0.9.1 (2022-12-XX) +Version 0.9.1 (2023-2-23) ======================== -* Adjust alert volume using ambient noise level -* Removed driver monitoring timer resetting on interaction if face detected and distracted -* New German translation thanks to Vrabetz and CzokNorris! +* New driving model + * 30% improved height estimation resulting in better driving performance for tall cars +* Driver monitoring: removed timer resetting on user interaction if distracted +* UI updates + * Adjust alert volume using ambient noise level + * Driver monitoring icon shows driver's head pose + * German translation thanks to Vrabetz and CzokNorris! * Chevrolet Bolt EV 2022-23 support thanks to JasonJShuler! * Genesis GV60 2023 support thanks to sunnyhaibin! * Hyundai Tucson 2022-23 support +* Kia K5 Hybrid 2020 support thanks to sunnyhaibin! +* Kia Niro Hybrid 2023 support thanks to sunnyhaibin! * Kia Sorento 2022-23 support thanks to sunnyhaibin! * Kia Sorento Plug-in Hybrid 2022 support thanks to sunnyhaibin! +* Toyota C-HR 2021 support thanks to eFiniLan! +* Toyota C-HR Hybrid 2022 support thanks to Korben00! +* Volkswagen Crafter and MAN TGE 2017-23 support thanks to jyoung8607! Version 0.9.0 (2022-11-21) ======================== diff --git a/SConstruct b/SConstruct index b148a9116a..31aa6ecced 100644 --- a/SConstruct +++ b/SConstruct @@ -282,7 +282,7 @@ Export('envCython') # Qt build environment qt_env = env.Clone() -qt_modules = ["Widgets", "Gui", "Core", "Network", "Concurrent", "Multimedia", "Quick", "Qml", "QuickWidgets", "Location", "Positioning", "DBus"] +qt_modules = ["Widgets", "Gui", "Core", "Network", "Concurrent", "Multimedia", "Quick", "Qml", "QuickWidgets", "Location", "Positioning", "DBus", "Xml"] qt_libs = [] if arch == "Darwin": @@ -311,6 +311,7 @@ else: qt_libs = [f"Qt5{m}" for m in qt_modules] if arch == "larch64": qt_libs += ["GLESv2", "wayland-client"] + qt_env.PrependENVPath('PATH', Dir("#third_party/qt5/larch64/bin/").abspath) elif arch != "Darwin": qt_libs += ["GL"] @@ -387,10 +388,10 @@ rednose_config = { if arch != "larch64": rednose_config['to_build'].update({ 'loc_4': ('#selfdrive/locationd/models/loc_kf.py', True, [], rednose_deps), + 'lane': ('#selfdrive/locationd/models/lane_kf.py', True, [], rednose_deps), 'pos_computer_4': ('#rednose/helpers/lst_sq_computer.py', False, [], []), 'pos_computer_5': ('#rednose/helpers/lst_sq_computer.py', False, [], []), 'feature_handler_5': ('#rednose/helpers/feature_handler.py', False, [], []), - 'lane': ('#xx/pipeline/lib/ekf/lane_kf.py', True, [], rednose_deps), }) Export('rednose_config') diff --git a/body b/body index dc780f858c..e1805f65ee 160000 --- a/body +++ b/body @@ -1 +1 @@ -Subproject commit dc780f858c1ef641471d09b72569e199e3e10acb +Subproject commit e1805f65ee75fab4454c21eda8b42b49d4bdc48f diff --git a/cereal b/cereal index c0d9abf6f7..fa3e77b7c8 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit c0d9abf6f7c7de140c41af10e322e226d900ef99 +Subproject commit fa3e77b7c8eee8752f19427b34adcb1ae5c70ec5 diff --git a/common/SConscript b/common/SConscript index 8aee6f42a7..5d6170611f 100644 --- a/common/SConscript +++ b/common/SConscript @@ -10,11 +10,13 @@ common_libs = [ 'statlog.cc', 'swaglog.cc', 'util.cc', - 'gpio.cc', 'i2c.cc', 'watchdog.cc', ] +if arch != "Darwin": + common_libs.append('gpio.cc') + _common = fxn('common', common_libs, LIBS="json11") files = [ diff --git a/common/gpio.cc b/common/gpio.cc index 9f5c211a4b..aa65c0bd9d 100644 --- a/common/gpio.cc +++ b/common/gpio.cc @@ -1,5 +1,20 @@ #include "common/gpio.h" +#ifdef __APPLE__ +int gpio_init(int pin_nr, bool output) { + return 0; +} + +int gpio_set(int pin_nr, bool high) { + return 0; +} + +int gpiochip_get_ro_value_fd(const char* consumer_label, int gpiochiop_id, int pin_nr) { + return 0; +} + +#else + #include #include @@ -63,3 +78,5 @@ int gpiochip_get_ro_value_fd(const char* consumer_label, int gpiochiop_id, int p close(fd); return rq.fd; } + +#endif diff --git a/common/params.cc b/common/params.cc index 8f6532bc79..db5e5e700d 100644 --- a/common/params.cc +++ b/common/params.cc @@ -104,13 +104,14 @@ std::unordered_map keys = { {"DisablePowerDown", PERSISTENT}, {"ExperimentalMode", PERSISTENT}, {"ExperimentalModeConfirmed", PERSISTENT}, - {"ExperimentalLongitudinalEnabled", PERSISTENT}, // WARNING: THIS MAY DISABLE AEB + {"ExperimentalLongitudinalEnabled", PERSISTENT}, {"DisableUpdates", PERSISTENT}, {"DisengageOnAccelerator", PERSISTENT}, {"DongleId", PERSISTENT}, {"DoReboot", CLEAR_ON_MANAGER_START}, {"DoShutdown", CLEAR_ON_MANAGER_START}, {"DoUninstall", CLEAR_ON_MANAGER_START}, + {"FirmwareObdQueryDone", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_ON}, {"ForcePowerDown", CLEAR_ON_MANAGER_START}, {"GitBranch", PERSISTENT}, {"GitCommit", PERSISTENT}, @@ -134,6 +135,7 @@ std::unordered_map keys = { {"IsRhdDetected", PERSISTENT}, {"IsTakingSnapshot", CLEAR_ON_MANAGER_START}, {"IsTestedBranch", CLEAR_ON_MANAGER_START}, + {"IsReleaseBranch", CLEAR_ON_MANAGER_START}, {"IsUpdateAvailable", CLEAR_ON_MANAGER_START}, {"JoystickDebugMode", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_OFF}, {"LaikadEphemeris", PERSISTENT | DONT_LOG}, @@ -169,7 +171,7 @@ std::unordered_map keys = { {"Timezone", PERSISTENT}, {"TrainingVersion", PERSISTENT}, {"UbloxAvailable", PERSISTENT}, - {"UpdateAvailable", CLEAR_ON_MANAGER_START}, + {"UpdateAvailable", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_ON}, {"UpdateFailedCount", CLEAR_ON_MANAGER_START}, {"UpdaterState", CLEAR_ON_MANAGER_START}, {"UpdaterFetchAvailable", CLEAR_ON_MANAGER_START}, diff --git a/docs/CARS.md b/docs/CARS.md index 0d4c53ac01..e6b6bbee7d 100644 --- a/docs/CARS.md +++ b/docs/CARS.md @@ -4,7 +4,7 @@ A supported vehicle is one that just works when you install a comma three. All supported cars provide a better experience than any stock system. -# 223 Supported Cars +# 236 Supported Cars |Make|Model|Supported Package|ACC|No ACC accel below|No ALC below|Steering Torque|Resume from stop|Harness|Video| |---|---|---|:---:|:---:|:---:|:---:|:---:|:---:|:---:| @@ -17,11 +17,11 @@ A supported vehicle is one that just works when you install a comma three. All s |Audi|Q3 2019-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| |Audi|RS3 2018|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| |Audi|S3 2015-17|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| -|Cadillac|Escalade ESV 2016[3](#footnotes)|Adaptive Cruise Control (ACC) & LKAS|openpilot|0 mph|6 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|OBD-II|| +|Cadillac|Escalade ESV 2016[3](#footnotes)|Adaptive Cruise Control (ACC) & LKAS|openpilot|0 mph|7 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|OBD-II|| |Chevrolet|Bolt EUV 2022-23|Premier or Premier Redline Trim without Super Cruise Package|openpilot available[1](#footnotes)|3 mph|6 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|GM|| |Chevrolet|Bolt EV 2022-23|2LT Trim with Adaptive Cruise Control Package|openpilot available[1](#footnotes)|3 mph|6 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|GM|| |Chevrolet|Silverado 1500 2020-21|Safety Package II|openpilot available[1](#footnotes)|3 mph|6 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|GM|| -|Chevrolet|Volt 2017-18[3](#footnotes)|Adaptive Cruise Control (ACC)|openpilot|0 mph|6 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|OBD-II|| +|Chevrolet|Volt 2017-18[3](#footnotes)|Adaptive Cruise Control (ACC)|openpilot|0 mph|7 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|OBD-II|| |Chrysler|Pacifica 2017-18|Adaptive Cruise Control (ACC)|Stock|0 mph|9 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|FCA|| |Chrysler|Pacifica 2019-20|Adaptive Cruise Control (ACC)|Stock|0 mph|39 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|FCA|| |Chrysler|Pacifica 2021|All|Stock|0 mph|39 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|FCA|| @@ -30,11 +30,13 @@ A supported vehicle is one that just works when you install a comma three. All s |comma|body|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|None|| |Genesis|G70 2018-19|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai F|| |Genesis|G70 2020|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai F|| -|Genesis|G80 2017-19|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai H|| +|Genesis|G80 2017|All|Stock|19 mph|37 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai J|| +|Genesis|G80 2018-19|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai H|| |Genesis|G90 2017-18|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai C|| -|Genesis|GV60 2023[5](#footnotes)|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai K|| +|Genesis|GV60 (Advanced Trim) 2023[5](#footnotes)|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai A|| +|Genesis|GV60 (Performance Trim) 2023[5](#footnotes)|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai K|| |Genesis|GV70 2022-23[5](#footnotes)|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai L|| -|GMC|Acadia 2018[3](#footnotes)|Adaptive Cruise Control (ACC)|openpilot|0 mph|6 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|OBD-II|| +|GMC|Acadia 2018[3](#footnotes)|Adaptive Cruise Control (ACC)|openpilot|0 mph|7 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|OBD-II|| |GMC|Sierra 1500 2020-21|Driver Alert Package II|openpilot available[1](#footnotes)|3 mph|6 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|GM|| |Honda|Accord 2018-22|All|openpilot available[1](#footnotes)|0 mph|3 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Bosch A|| |Honda|Accord Hybrid 2018-22|All|openpilot available[1](#footnotes)|0 mph|3 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Bosch A|| @@ -57,7 +59,7 @@ A supported vehicle is one that just works when you install a comma three. All s |Honda|Pilot 2016-22|Honda Sensing|openpilot|25 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Nidec|| |Honda|Ridgeline 2017-22|Honda Sensing|openpilot|25 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Nidec|| |Hyundai|Elantra 2017-19|Smart Cruise Control (SCC)|Stock|19 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai B|| -|Hyundai|Elantra 2021-22|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai K|| +|Hyundai|Elantra 2021-23|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai K|| |Hyundai|Elantra GT 2017-19|Smart Cruise Control (SCC)|Stock|0 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai E|| |Hyundai|Elantra Hybrid 2021-23|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai K|| |Hyundai|Genesis 2015-16|Smart Cruise Control (SCC)|Stock|19 mph|37 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai J|| @@ -98,13 +100,16 @@ A supported vehicle is one that just works when you install a comma three. All s |Kia|EV6 (without HDA II) 2022[5](#footnotes)|Highway Driving Assist|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai L|| |Kia|Forte 2019-21|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai G|| |Kia|K5 2021-22|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai A|| +|Kia|K5 Hybrid 2020|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai A|| |Kia|Niro EV 2019|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai H|| |Kia|Niro EV 2020|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai F|| |Kia|Niro EV 2021|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai C|| |Kia|Niro EV 2022|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai H|| |Kia|Niro Hybrid 2021|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai F|| |Kia|Niro Hybrid 2022|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai H|| +|Kia|Niro Hybrid 2023[5](#footnotes)|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai A|| |Kia|Niro Plug-in Hybrid 2018-19|All|openpilot available[1](#footnotes)|10 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai C|| +|Kia|Niro Plug-in Hybrid 2020|All|openpilot available[1](#footnotes)|0 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai D|| |Kia|Optima 2017|Advanced Smart Cruise Control|Stock|0 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai B|| |Kia|Optima 2019-20|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai G|| |Kia|Seltos 2021|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai A|| @@ -114,13 +119,13 @@ A supported vehicle is one that just works when you install a comma three. All s |Kia|Sorento Plug-in Hybrid 2022-23[5](#footnotes)|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai A|| |Kia|Sportage 2023[5](#footnotes)|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai N|| |Kia|Sportage Hybrid 2023[5](#footnotes)|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai N|| -|Kia|Stinger 2018-20|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai C|| +|Kia|Stinger 2018-20|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai C|| |Kia|Stinger 2022|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai K|| |Kia|Telluride 2020-22|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai H|| |Lexus|CT Hybrid 2017-18|Lexus Safety System+|openpilot available[2](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota|| |Lexus|ES 2019-22|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota|| |Lexus|ES Hybrid 2017-18|Lexus Safety System+|openpilot available[2](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota|| -|Lexus|ES Hybrid 2019-22|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota|| +|Lexus|ES Hybrid 2019-23|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota|| |Lexus|IS 2017-19|All|Stock|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota|| |Lexus|NX 2018-19|All|openpilot available[2](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota|| |Lexus|NX 2020-21|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota|| @@ -134,6 +139,8 @@ A supported vehicle is one that just works when you install a comma three. All s |Lexus|RX Hybrid 2017-19|All|openpilot available[2](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota|| |Lexus|RX Hybrid 2020-21|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota|| |Lexus|UX Hybrid 2019-22|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota|| +|MAN|eTGE 2020-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|31 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| +|MAN|TGE 2017-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|31 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| |Mazda|CX-5 2022-23|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Mazda|| |Mazda|CX-9 2021-23|All|Stock|0 mph|28 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Mazda|| |Nissan|Altima 2019-20|ProPILOT Assist|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Nissan B|| @@ -168,8 +175,10 @@ A supported vehicle is one that just works when you install a comma three. All s |Toyota|Avalon 2022|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota|| |Toyota|Avalon Hybrid 2019-21|All|openpilot available[2](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota|| |Toyota|Avalon Hybrid 2022|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota|| -|Toyota|C-HR 2017-21|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota|| +|Toyota|C-HR 2017-20|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota|| +|Toyota|C-HR 2021|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota|| |Toyota|C-HR Hybrid 2017-19|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota|| +|Toyota|C-HR Hybrid 2022|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota|| |Toyota|Camry 2018-20|All|Stock|0 mph[6](#footnotes)|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota|| |Toyota|Camry 2021-22|All|openpilot|0 mph[6](#footnotes)|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota|| |Toyota|Camry Hybrid 2018-20|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota|| @@ -180,6 +189,7 @@ A supported vehicle is one that just works when you install a comma three. All s |Toyota|Corolla Cross Hybrid (Non-US only) 2020-22|All|openpilot|17 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota|| |Toyota|Corolla Hatchback 2019-22|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota|| |Toyota|Corolla Hybrid 2020-22|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota|| +|Toyota|Corolla Hybrid (Non-US only) 2020-23|All|openpilot|17 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota|| |Toyota|Highlander 2017-19|All|openpilot available[2](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota|| |Toyota|Highlander 2020-23|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota|| |Toyota|Highlander Hybrid 2017-19|All|openpilot available[2](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota|| @@ -208,6 +218,8 @@ A supported vehicle is one that just works when you install a comma three. All s |Volkswagen|California 2021|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|31 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| |Volkswagen|Caravelle 2020|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|31 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| |Volkswagen|CC 2018-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| +|Volkswagen|Crafter 2017-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|31 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| +|Volkswagen|e-Crafter 2018-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|31 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| |Volkswagen|e-Golf 2014-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| |Volkswagen|Golf 2015-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| |Volkswagen|Golf Alltrack 2015-19|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| @@ -216,6 +228,7 @@ A supported vehicle is one that just works when you install a comma three. All s |Volkswagen|Golf GTI 2015-21|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| |Volkswagen|Golf R 2015-19|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| |Volkswagen|Golf SportsVan 2015-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| +|Volkswagen|Grand California 2019-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|31 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| |Volkswagen|Jetta 2018-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| |Volkswagen|Jetta GLI 2021-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| |Volkswagen|Passat 2015-22[8](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| @@ -229,7 +242,7 @@ A supported vehicle is one that just works when you install a comma three. All s |Volkswagen|Teramont 2018-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| |Volkswagen|Teramont Cross Sport 2021-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| |Volkswagen|Teramont X 2021-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| -|Volkswagen|Tiguan 2019-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| +|Volkswagen|Tiguan 2018-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| |Volkswagen|Touran 2017|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| diff --git a/docs/overview.rst b/docs/overview.rst index cda51ba3d4..cc4c582155 100644 --- a/docs/overview.rst +++ b/docs/overview.rst @@ -77,3 +77,4 @@ tools Simulator tools/ssh/README.md Webcam + tools/cabana/README.md diff --git a/laika_repo b/laika_repo index 73bf110ae0..278b44ba8c 160000 --- a/laika_repo +++ b/laika_repo @@ -1 +1 @@ -Subproject commit 73bf110ae0093ad86755bf5eb6a03e46ff5c239d +Subproject commit 278b44ba8c2dec28adc5b30cce13dabc195f15fc diff --git a/opendbc b/opendbc index d585a9bf29..510bfc0695 160000 --- a/opendbc +++ b/opendbc @@ -1 +1 @@ -Subproject commit d585a9bf2908b2c83bf02b567b9e1f5bfc587a01 +Subproject commit 510bfc06954e31257f8d8de17adf92f9a68a1b71 diff --git a/panda b/panda index 11d90f9e78..d15250cb14 160000 --- a/panda +++ b/panda @@ -1 +1 @@ -Subproject commit 11d90f9e78b1c070e44e02d5d8c2b18790617324 +Subproject commit d15250cb1454292c6f1217c79642b9ffd93e7595 diff --git a/release/build_release.sh b/release/build_release.sh index 80106eefb2..60f81fce06 100755 --- a/release/build_release.sh +++ b/release/build_release.sh @@ -11,15 +11,19 @@ SOURCE_DIR="$(git rev-parse --show-toplevel)" if [ -f /TICI ]; then FILES_SRC="release/files_tici" - RELEASE_BRANCH=release3-staging - DASHCAM_BRANCH=dashcam3-staging else - exit 0 + echo "no release files set" + exit 1 +fi + +if [ -z "$RELEASE_BRANCH" ]; then + echo "RELEASE_BRANCH is not set" + exit 1 fi + # set git identity source $DIR/identity.sh -export GIT_SSH_COMMAND="ssh -i /data/gitkey" echo "[-] Setting up repo T=$SECONDS" rm -rf $BUILD_DIR @@ -27,7 +31,6 @@ mkdir -p $BUILD_DIR cd $BUILD_DIR git init git remote add origin git@github.com:commaai/openpilot.git -git fetch origin $RELEASE_BRANCH git checkout --orphan $RELEASE_BRANCH # do the files copy @@ -48,7 +51,6 @@ echo "#define COMMA_VERSION \"$VERSION-release\"" > common/version.h echo "[-] committing version $VERSION T=$SECONDS" git add -f . git commit -a -m "openpilot v$VERSION release" -git branch --set-upstream-to=origin/$RELEASE_BRANCH # Build panda firmware pushd panda/ @@ -105,10 +107,12 @@ RELEASE=1 selfdrive/test/test_onroad.py selfdrive/car/tests/test_car_interfaces.py rm -rf $TEST_FILES -if [ ! -z "$PUSH" ]; then - echo "[-] pushing T=$SECONDS" - git push -f origin $RELEASE_BRANCH +if [ ! -z "$RELEASE_BRANCH" ]; then + echo "[-] pushing release T=$SECONDS" + git push -f origin $RELEASE_BRANCH:$RELEASE_BRANCH +fi +if [ ! -z "$DASHCAM_BRANCH" ]; then # Create dashcam git rm selfdrive/car/*/carcontroller.py git commit -m "create dashcam release from release" diff --git a/release/check-submodules.sh b/release/check-submodules.sh index 182042e6b4..bc85a43c57 100755 --- a/release/check-submodules.sh +++ b/release/check-submodules.sh @@ -1,7 +1,7 @@ #!/bin/bash while read hash submodule ref; do - git -C $submodule fetch --depth 100 origin master + git -C $submodule fetch --depth 200 origin master git -C $submodule branch -r --contains $hash | grep "origin/master" if [ "$?" -eq 0 ]; then echo "$submodule ok" diff --git a/release/files_common b/release/files_common index 3d07924a91..5b310d5c80 100644 --- a/release/files_common +++ b/release/files_common @@ -225,10 +225,7 @@ selfdrive/locationd/SConscript selfdrive/locationd/ubloxd.cc selfdrive/locationd/ublox_msg.cc selfdrive/locationd/ublox_msg.h -selfdrive/locationd/generated/ubx.cpp -selfdrive/locationd/generated/ubx.h -selfdrive/locationd/generated/gps.cpp -selfdrive/locationd/generated/gps.h +selfdrive/locationd/generated/* selfdrive/locationd/laikad.py selfdrive/locationd/locationd.h @@ -320,6 +317,12 @@ selfdrive/ui/qt/widgets/*.cc selfdrive/ui/qt/widgets/*.h selfdrive/ui/qt/maps/*.cc selfdrive/ui/qt/maps/*.h +selfdrive/ui/qt/setup/*.cc +selfdrive/ui/qt/setup/*.h + +selfdrive/ui/installer/*.cc +selfdrive/ui/installer/*.h +selfdrive/ui/installer/*.cc system/camerad/SConscript system/camerad/main.cc @@ -436,6 +439,7 @@ third_party/acados/larch64/** third_party/acados/include/** third_party/acados/acados_template/** +third_party/bootstrap/** third_party/qt5/larch64/bin/** scripts/update_now.sh @@ -573,17 +577,13 @@ opendbc/tesla_can.dbc opendbc/tesla_radar.dbc opendbc/tesla_powertrain.dbc -tinygrad_repo/openpilot/compile.py tinygrad_repo/accel/opencl/* +tinygrad_repo/tinygrad/llops/ops_opencl.py +tinygrad_repo/openpilot/compile.py tinygrad_repo/extra/onnx.py tinygrad_repo/extra/thneed.py tinygrad_repo/extra/utils.py tinygrad_repo/tinygrad/llops/ops_gpu.py -tinygrad_repo/tinygrad/llops/ops_opencl.py -tinygrad_repo/tinygrad/helpers.py -tinygrad_repo/tinygrad/mlops.py -tinygrad_repo/tinygrad/ops.py -tinygrad_repo/tinygrad/shapetracker.py -tinygrad_repo/tinygrad/tensor.py -tinygrad_repo/tinygrad/nn/__init__.py -tinygrad_repo/tinygrad/nn/optim.py +tinygrad_repo/tinygrad/shape/* +tinygrad_repo/tinygrad/nn/* +tinygrad_repo/tinygrad/*.py diff --git a/scripts/cell.sh b/scripts/cell.sh index cae701eccc..3623fe5b16 100755 --- a/scripts/cell.sh +++ b/scripts/cell.sh @@ -1,5 +1,3 @@ #!/usr/bin/bash - -nmcli connection modify --temporary lte ipv4.route-metric 1 -nmcli connection modify --temporary lte ipv6.route-metric 1 -nmcli con up lte +nmcli connection modify --temporary lte ipv4.route-metric 1 ipv6.route-metric 1 +nmcli con up --wait --ask lte diff --git a/selfdrive/assets/assets.qrc b/selfdrive/assets/assets.qrc index 39be41aa65..79a1a1e272 100644 --- a/selfdrive/assets/assets.qrc +++ b/selfdrive/assets/assets.qrc @@ -1,5 +1,6 @@ + ../../third_party/bootstrap/bootstrap-icons.svg img_continue_triangle.svg img_circled_check.svg img_circled_slash.svg diff --git a/selfdrive/assets/img_driver_face.png b/selfdrive/assets/img_driver_face.png index ddde478cd7..03765a0376 100644 Binary files a/selfdrive/assets/img_driver_face.png and b/selfdrive/assets/img_driver_face.png differ diff --git a/selfdrive/boardd/boardd.cc b/selfdrive/boardd/boardd.cc index 1f1249194d..0473d3488c 100644 --- a/selfdrive/boardd/boardd.cc +++ b/selfdrive/boardd/boardd.cc @@ -7,6 +7,7 @@ #include #include +#include #include #include #include @@ -48,8 +49,8 @@ #define MAX_IR_POWER 0.5f #define MIN_IR_POWER 0.0f -#define CUTOFF_IL 200 -#define SATURATE_IL 1600 +#define CUTOFF_IL 400 +#define SATURATE_IL 1000 #define NIBBLE_TO_HEX(n) ((n) < 10 ? (n) + '0' : ((n) - 10) + 'a') using namespace std::chrono_literals; @@ -105,6 +106,8 @@ void sync_time(Panda *panda, SyncTimeDir dir) { bool safety_setter_thread(std::vector pandas) { LOGD("Starting safety setter thread"); + Params p; + // there should be at least one panda connected if (pandas.size() == 0) { return false; @@ -116,25 +119,20 @@ bool safety_setter_thread(std::vector pandas) { pandas[i]->set_safety_model(cereal::CarParams::SafetyModel::ELM327, safety_param); } - Params p = Params(); - - // wait for VIN to be read + // wait for FW query at OBD port to finish while (true) { if (do_exit || !check_all_connected(pandas) || !ignition) { return false; } - std::string value_vin = p.get("CarVin"); - if (value_vin.size() > 0) { - // sanity check VIN format - assert(value_vin.size() == 17); - LOGW("got CarVin %s", value_vin.c_str()); + if (p.getBool("FirmwareObdQueryDone")) { + LOGW("finished FW query at OBD port"); break; } util::sleep_for(20); } - // set to ELM327 for ECU knockouts + // set to ELM327 to finish fingerprinting and for potential ECU knockouts for (Panda *panda : pandas) { panda->set_safety_model(cereal::CarParams::SafetyModel::ELM327, 1U); } @@ -225,9 +223,9 @@ void can_send_thread(std::vector pandas, bool fake_send) { //Dont send if older than 1 second if ((nanos_since_boot() - event.getLogMonoTime() < 1e9) && !fake_send) { for (const auto& panda : pandas) { - LOGT("sending sendcan to panda: %s", (panda->hw_serial).c_str()); + LOGT("sending sendcan to panda: %s", (panda->hw_serial()).c_str()); panda->can_send(event.getSendcan()); - LOGT("sendcan sent to panda: %s", (panda->hw_serial).c_str()); + LOGT("sendcan sent to panda: %s", (panda->hw_serial()).c_str()); } } } @@ -540,9 +538,8 @@ void peripheral_control_thread(Panda *panda, bool no_fan_control) { if (sm.updated("driverCameraState")) { auto event = sm["driverCameraState"]; int cur_integ_lines = event.getDriverCameraState().getIntegLines(); - float cur_gain = event.getDriverCameraState().getGain(); - cur_integ_lines = integ_lines_filter.update(cur_integ_lines * cur_gain); + cur_integ_lines = integ_lines_filter.update(cur_integ_lines); last_front_frame_t = event.getLogMonoTime(); if (cur_integ_lines <= CUTOFF_IL) { diff --git a/selfdrive/boardd/panda.cc b/selfdrive/boardd/panda.cc index d4d53aa230..647a0d9c78 100644 --- a/selfdrive/boardd/panda.cc +++ b/selfdrive/boardd/panda.cc @@ -10,11 +10,13 @@ #include "common/util.h" Panda::Panda(std::string serial, uint32_t bus_offset) : bus_offset(bus_offset) { - // TODO: support SPI here one day... - if (serial.find("spi") != std::string::npos) { - handle = std::make_unique(serial); - } else { + // try USB first, then SPI + try { handle = std::make_unique(serial); + } catch (std::exception &e) { +#ifndef __APPLE__ + handle = std::make_unique(serial); +#endif } hw_type = get_hw_type(); @@ -39,8 +41,22 @@ bool Panda::comms_healthy() { return handle->comms_healthy; } +std::string Panda::hw_serial() { + return handle->hw_serial; +} + std::vector Panda::list() { - return PandaUsbHandle::list(); + std::vector serials = PandaUsbHandle::list(); + +#ifndef __APPLE__ + for (auto s : PandaSpiHandle::list()) { + if (std::find(serials.begin(), serials.end(), s) == serials.end()) { + serials.push_back(s); + } + } +#endif + + return serials; } void Panda::set_safety_model(cereal::CarParams::SafetyModel safety_model, uint16_t safety_param) { @@ -220,6 +236,9 @@ void Panda::can_send(capnp::List::Reader can_data_list) { } bool Panda::can_receive(std::vector& out_vec) { + // Check if enough space left in buffer to store RECV_SIZE data + assert(receive_buffer_size + RECV_SIZE <= sizeof(receive_buffer)); + int recv = handle->bulk_read(0x81, &receive_buffer[receive_buffer_size], RECV_SIZE); if (!comms_healthy()) { return false; @@ -262,6 +281,7 @@ bool Panda::unpack_can_buffer(uint8_t *data, uint32_t &size, std::vector list(); diff --git a/selfdrive/boardd/panda_comms.cc b/selfdrive/boardd/panda_comms.cc index e73cb69318..120d2f67d5 100644 --- a/selfdrive/boardd/panda_comms.cc +++ b/selfdrive/boardd/panda_comms.cc @@ -44,7 +44,7 @@ PandaUsbHandle::PandaUsbHandle(std::string serial) : PandaCommsHandle(serial) { ret = libusb_get_string_descriptor_ascii(dev_handle, desc.iSerialNumber, desc_serial, std::size(desc_serial)); if (ret < 0) { goto fail; } - auto hw_serial = std::string((char *)desc_serial, ret); + hw_serial = std::string((char *)desc_serial, ret); if (serial.empty() || serial == hw_serial) { break; } diff --git a/selfdrive/boardd/panda_comms.h b/selfdrive/boardd/panda_comms.h index bd262dfa0e..506b96b372 100644 --- a/selfdrive/boardd/panda_comms.h +++ b/selfdrive/boardd/panda_comms.h @@ -5,7 +5,9 @@ #include #include +#ifndef __APPLE__ #include +#endif #include @@ -21,6 +23,7 @@ public: virtual ~PandaCommsHandle() {}; virtual void cleanup() = 0; + std::string hw_serial; std::atomic connected = true; std::atomic comms_healthy = true; static std::vector list(); @@ -30,9 +33,6 @@ public: virtual int control_read(uint8_t request, uint16_t param1, uint16_t param2, unsigned char *data, uint16_t length, unsigned int timeout=TIMEOUT) = 0; virtual int bulk_write(unsigned char endpoint, unsigned char* data, int length, unsigned int timeout=TIMEOUT) = 0; virtual int bulk_read(unsigned char endpoint, unsigned char* data, int length, unsigned int timeout=TIMEOUT) = 0; - -protected: - std::recursive_mutex hw_lock; }; class PandaUsbHandle : public PandaCommsHandle { @@ -50,9 +50,11 @@ public: private: libusb_context *ctx = NULL; libusb_device_handle *dev_handle = NULL; + std::recursive_mutex hw_lock; void handle_usb_issue(int err, const char func[]); }; +#ifndef __APPLE__ class PandaSpiHandle : public PandaCommsHandle { public: PandaSpiHandle(std::string serial); @@ -69,9 +71,11 @@ private: int spi_fd = -1; uint8_t tx_buf[SPI_BUF_SIZE]; uint8_t rx_buf[SPI_BUF_SIZE]; + inline static std::recursive_mutex hw_lock; int wait_for_ack(spi_ioc_transfer &transfer, uint8_t ack); int bulk_transfer(uint8_t endpoint, uint8_t *tx_data, uint16_t tx_len, uint8_t *rx_data, uint16_t rx_len); int spi_transfer(uint8_t endpoint, uint8_t *tx_data, uint16_t tx_len, uint8_t *rx_data, uint16_t max_rx_len); int spi_transfer_retry(uint8_t endpoint, uint8_t *tx_data, uint16_t tx_len, uint8_t *rx_data, uint16_t max_rx_len); }; +#endif diff --git a/selfdrive/boardd/pandad.py b/selfdrive/boardd/pandad.py index 971756002b..f61d9ee1a6 100755 --- a/selfdrive/boardd/pandad.py +++ b/selfdrive/boardd/pandad.py @@ -7,7 +7,7 @@ import subprocess from typing import List, NoReturn from functools import cmp_to_key -from panda import DEFAULT_FW_FN, DEFAULT_H7_FW_FN, MCU_TYPE_H7, Panda, PandaDFU +from panda import Panda, PandaDFU from common.basedir import BASEDIR from common.params import Params from system.hardware import HARDWARE @@ -15,10 +15,8 @@ from system.swaglog import cloudlog 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: - return Panda.get_signature_from_firmware(fn) + return Panda.get_signature_from_firmware(panda.get_mcu_type().config.app_path) except Exception: cloudlog.exception("Error computing expected signature") return b"" diff --git a/selfdrive/boardd/spi.cc b/selfdrive/boardd/spi.cc index 717b6ce820..9a10e30f95 100644 --- a/selfdrive/boardd/spi.cc +++ b/selfdrive/boardd/spi.cc @@ -1,9 +1,13 @@ +#ifndef __APPLE__ +#include #include #include #include #include #include +#include +#include #include "common/util.h" #include "common/timing.h" @@ -27,41 +31,77 @@ struct __attribute__((packed)) spi_header { const int SPI_MAX_RETRIES = 5; const int SPI_ACK_TIMEOUT = 50; // milliseconds +const std::string SPI_DEVICE = "/dev/spidev0.0"; + +class LockEx { +public: + LockEx(int fd, std::recursive_mutex &m) : fd(fd), m(m) { + m.lock(); + flock(fd, LOCK_EX); + }; + + ~LockEx() { + m.unlock(); + flock(fd, LOCK_UN); + } + +private: + int fd; + std::recursive_mutex &m; +}; PandaSpiHandle::PandaSpiHandle(std::string serial) : PandaCommsHandle(serial) { - LOGD("opening SPI panda: %s", serial.c_str()); + int ret; + const int uid_len = 12; + uint8_t uid[uid_len] = {0}; - int err; uint32_t spi_mode = SPI_MODE_0; uint32_t spi_speed = 30000000; uint8_t spi_bits_per_word = 8; - spi_fd = open(serial.c_str(), O_RDWR); + spi_fd = open(SPI_DEVICE.c_str(), O_RDWR); if (spi_fd < 0) { - LOGE("failed opening SPI device %d", err); + LOGE("failed opening SPI device %d", spi_fd); goto fail; } // SPI settings - err = util::safe_ioctl(spi_fd, SPI_IOC_WR_MODE, &spi_mode); - if (err < 0) { - LOGE("failed setting SPI mode %d", err); + ret = util::safe_ioctl(spi_fd, SPI_IOC_WR_MODE, &spi_mode); + if (ret < 0) { + LOGE("failed setting SPI mode %d", ret); goto fail; } - err = util::safe_ioctl(spi_fd, SPI_IOC_WR_MAX_SPEED_HZ, &spi_speed); - if (err < 0) { + ret = util::safe_ioctl(spi_fd, SPI_IOC_WR_MAX_SPEED_HZ, &spi_speed); + if (ret < 0) { LOGE("failed setting SPI speed"); goto fail; } - err = util::safe_ioctl(spi_fd, SPI_IOC_WR_BITS_PER_WORD, &spi_bits_per_word); - if (err < 0) { + ret = util::safe_ioctl(spi_fd, SPI_IOC_WR_BITS_PER_WORD, &spi_bits_per_word); + if (ret < 0) { LOGE("failed setting SPI bits per word"); goto fail; } + // get hw UID/serial + ret = control_read(0xc3, 0, 0, uid, uid_len); + if (ret == uid_len) { + std::stringstream stream; + for (int i = 0; i < uid_len; i++) { + stream << std::hex << std::setw(2) << std::setfill('0') << int(uid[i]); + } + hw_serial = stream.str(); + } else { + LOGD("failed to get serial %d", ret); + goto fail; + } + + if (!serial.empty() && (serial != hw_serial)) { + goto fail; + } + return; fail: @@ -84,6 +124,7 @@ void PandaSpiHandle::cleanup() { int PandaSpiHandle::control_write(uint8_t request, uint16_t param1, uint16_t param2, unsigned int timeout) { + LockEx lock(spi_fd, hw_lock); ControlPacket_t packet = { .request = request, .param1 = param1, @@ -94,6 +135,7 @@ int PandaSpiHandle::control_write(uint8_t request, uint16_t param1, uint16_t par } int PandaSpiHandle::control_read(uint8_t request, uint16_t param1, uint16_t param2, unsigned char *data, uint16_t length, unsigned int timeout) { + LockEx lock(spi_fd, hw_lock); ControlPacket_t packet = { .request = request, .param1 = param1, @@ -104,15 +146,15 @@ int PandaSpiHandle::control_read(uint8_t request, uint16_t param1, uint16_t para } int PandaSpiHandle::bulk_write(unsigned char endpoint, unsigned char* data, int length, unsigned int timeout) { + LockEx lock(spi_fd, hw_lock); return bulk_transfer(endpoint, data, length, NULL, 0); } int PandaSpiHandle::bulk_read(unsigned char endpoint, unsigned char* data, int length, unsigned int timeout) { + LockEx lock(spi_fd, hw_lock); return bulk_transfer(endpoint, NULL, 0, data, length); } int PandaSpiHandle::bulk_transfer(uint8_t endpoint, uint8_t *tx_data, uint16_t tx_len, uint8_t *rx_data, uint16_t rx_len) { - std::lock_guard lk(hw_lock); - const int xfer_size = 0x40 * 15; int ret = 0; @@ -143,7 +185,12 @@ int PandaSpiHandle::bulk_transfer(uint8_t endpoint, uint8_t *tx_data, uint16_t t } std::vector PandaSpiHandle::list() { - // TODO: list all pandas available over SPI + try { + PandaSpiHandle sh(""); + return {sh.hw_serial}; + } catch (std::exception &e) { + // no panda on SPI + } return {}; } @@ -167,7 +214,6 @@ int PandaSpiHandle::spi_transfer_retry(uint8_t endpoint, uint8_t *tx_data, uint1 int ret; int count = SPI_MAX_RETRIES; - std::lock_guard lk(hw_lock); do { // TODO: handle error ret = spi_transfer(endpoint, tx_data, tx_len, rx_data, max_rx_len); @@ -195,7 +241,7 @@ int PandaSpiHandle::wait_for_ack(spi_ioc_transfer &transfer, uint8_t ack) { // handle timeout if (millis_since_boot() - start_millis > SPI_ACK_TIMEOUT) { - LOGE("SPI: timed out waiting for ACK"); + LOGD("SPI: timed out waiting for ACK"); return -1; } } @@ -270,7 +316,10 @@ int PandaSpiHandle::spi_transfer(uint8_t endpoint, uint8_t *tx_data, uint16_t tx goto transfer_fail; } rx_data_len = *(uint16_t *)(rx_buf+1); - assert(rx_data_len < SPI_BUF_SIZE); + if (rx_data_len >= SPI_BUF_SIZE) { + LOGE("SPI: RX data len larger than buf size %d", rx_data_len); + goto transfer_fail; + } // Read data transfer.len = rx_data_len + 1; @@ -294,3 +343,4 @@ int PandaSpiHandle::spi_transfer(uint8_t endpoint, uint8_t *tx_data, uint16_t tx transfer_fail: return ret; } +#endif diff --git a/selfdrive/boardd/tests/test_boardd_loopback.py b/selfdrive/boardd/tests/test_boardd_loopback.py index e9bbcb4586..b8ebbd88a3 100755 --- a/selfdrive/boardd/tests/test_boardd_loopback.py +++ b/selfdrive/boardd/tests/test_boardd_loopback.py @@ -51,7 +51,7 @@ class TestBoardd(unittest.TestCase): cp.safetyConfigs = [safety_config]*num_pandas params = Params() - params.put("CarVin", b"0"*17) + params.put_bool("FirmwareObdQueryDone", True) params.put_bool("ControlsReady", True) params.put("CarParams", cp.to_bytes()) diff --git a/selfdrive/car/body/carcontroller.py b/selfdrive/car/body/carcontroller.py index 00673a7e28..bcaf6f6f93 100644 --- a/selfdrive/car/body/carcontroller.py +++ b/selfdrive/car/body/carcontroller.py @@ -35,7 +35,7 @@ class CarController: torque -= deadband return torque - def update(self, CC, CS): + def update(self, CC, CS, now_nanos): torque_l = 0 torque_r = 0 diff --git a/selfdrive/car/body/interface.py b/selfdrive/car/body/interface.py index bc5b36e2ee..850a3538ad 100644 --- a/selfdrive/car/body/interface.py +++ b/selfdrive/car/body/interface.py @@ -24,7 +24,7 @@ class CarInterface(CarInterfaceBase): ret.wheelSpeedFactor = SPEED_FROM_RPM ret.centerToFront = ret.wheelbase * 0.44 - ret.radarOffCan = True + ret.radarUnavailable = True ret.openpilotLongitudinalControl = True ret.steerControlType = car.CarParams.SteerControlType.angle @@ -43,5 +43,5 @@ class CarInterface(CarInterfaceBase): return ret - def apply(self, c): - return self.CC.update(c, self.CS) + def apply(self, c, now_nanos): + return self.CC.update(c, self.CS, now_nanos) diff --git a/selfdrive/car/car_helpers.py b/selfdrive/car/car_helpers.py index 61e7a3d55d..4ccce979d3 100644 --- a/selfdrive/car/car_helpers.py +++ b/selfdrive/car/car_helpers.py @@ -99,7 +99,7 @@ def fingerprint(logcan, sendcan, num_pandas): else: cloudlog.warning("Getting VIN & FW versions") vin_rx_addr, vin = get_vin(logcan, sendcan, bus) - ecu_rx_addrs = get_present_ecus(logcan, sendcan) + ecu_rx_addrs = get_present_ecus(logcan, sendcan, num_pandas=num_pandas) car_fw = get_fw_versions_ordered(logcan, sendcan, ecu_rx_addrs, num_pandas=num_pandas) cached = False @@ -113,7 +113,10 @@ def fingerprint(logcan, sendcan, num_pandas): cloudlog.event("Malformed VIN", vin=vin, error=True) vin = VIN_UNKNOWN cloudlog.warning("VIN %s", vin) - Params().put("CarVin", vin) + + params = Params() + params.put("CarVin", vin) + params.put_bool("FirmwareObdQueryDone", True) finger = gen_empty_fingerprint() candidate_cars = {i: all_legacy_fingerprint_cars() for i in [0, 1]} # attempt fingerprint on both bus 0 and 1 @@ -173,17 +176,15 @@ def fingerprint(logcan, sendcan, num_pandas): return car_fingerprint, finger, vin, car_fw, source, exact_match -def get_car(logcan, sendcan, num_pandas=1): +def get_car(logcan, sendcan, experimental_long_allowed, num_pandas=1): candidate, fingerprints, vin, car_fw, source, exact_match = fingerprint(logcan, sendcan, num_pandas) if candidate is None: cloudlog.warning("car doesn't match any fingerprints: %r", fingerprints) candidate = "mock" - experimental_long = Params().get_bool("ExperimentalLongitudinalEnabled") - CarInterface, CarController, CarState = interfaces[candidate] - CP = CarInterface.get_params(candidate, fingerprints, car_fw, experimental_long) + CP = CarInterface.get_params(candidate, fingerprints, car_fw, experimental_long_allowed) CP.carVin = vin CP.carFw = car_fw CP.fingerprintSource = source diff --git a/selfdrive/car/chrysler/carcontroller.py b/selfdrive/car/chrysler/carcontroller.py index ba6aaf8250..20a44bce21 100644 --- a/selfdrive/car/chrysler/carcontroller.py +++ b/selfdrive/car/chrysler/carcontroller.py @@ -19,7 +19,7 @@ class CarController: self.packer = CANPacker(dbc_name) self.params = CarControllerParams(CP) - def update(self, CC, CS): + def update(self, CC, CS, now_nanos): can_sends = [] lkas_active = CC.latActive and self.lkas_control_bit_prev diff --git a/selfdrive/car/chrysler/carstate.py b/selfdrive/car/chrysler/carstate.py index 0f0d30782a..fdc5aa338a 100644 --- a/selfdrive/car/chrysler/carstate.py +++ b/selfdrive/car/chrysler/carstate.py @@ -81,8 +81,9 @@ class CarState(CarStateBase): if self.CP.carFingerprint in RAM_CARS: self.auto_high_beam = cp_cam.vl["DAS_6"]['AUTO_HIGH_BEAM_ON'] # Auto High Beam isn't Located in this message on chrysler or jeep currently located in 729 message - ret.steerFaultTemporary = cp.vl["EPS_3"]["DASM_FAULT"] == 1 + ret.steerFaultTemporary = cp.vl["EPS_3"]["DASM_FAULT"] == 1 else: + ret.steerFaultTemporary = cp.vl["EPS_2"]["LKAS_TEMPORARY_FAULT"] == 1 ret.steerFaultPermanent = cp.vl["EPS_2"]["LKAS_STATE"] == 4 # blindspot sensors @@ -135,6 +136,7 @@ class CarState(CarStateBase): ("COUNTER", "EPS_2",), ("COLUMN_TORQUE", "EPS_2"), ("EPS_TORQUE_MOTOR", "EPS_2"), + ("LKAS_TEMPORARY_FAULT", "EPS_2"), ("LKAS_STATE", "EPS_2"), ("COUNTER", "CRUISE_BUTTONS"), ] diff --git a/selfdrive/car/chrysler/interface.py b/selfdrive/car/chrysler/interface.py index 2f058165ac..4b22c478a7 100755 --- a/selfdrive/car/chrysler/interface.py +++ b/selfdrive/car/chrysler/interface.py @@ -2,7 +2,7 @@ from cereal import car from panda import Panda from selfdrive.car import STD_CARGO_KG, get_safety_config -from selfdrive.car.chrysler.values import CAR, DBC, RAM_HD, RAM_DT, RAM_CARS, ChryslerFlags +from selfdrive.car.chrysler.values import CAR, RAM_HD, RAM_DT, RAM_CARS, CHRYSLER_OLD_TUNING_BLACKLIST, ChryslerFlags from selfdrive.car.interfaces import CarInterfaceBase @@ -12,7 +12,7 @@ class CarInterface(CarInterfaceBase): ret.carName = "chrysler" ret.dashcamOnly = candidate in RAM_HD - ret.radarOffCan = DBC[candidate]['radar'] is None + ret.radarUnavailable = True # DBC[candidate]['radar'] is None ret.steerActuatorDelay = 0.1 ret.steerLimitTimer = 0.4 @@ -23,7 +23,11 @@ class CarInterface(CarInterfaceBase): elif candidate in RAM_DT: ret.safetyConfigs[0].safetyParam |= Panda.FLAG_CHRYSLER_RAM_DT + if candidate in CHRYSLER_OLD_TUNING_BLACKLIST: + ret.safetyConfigs[0].safetyParam |= Panda.FLAG_CHRYSLER_LOWER_RATE + ret.minSteerSpeed = 3.8 # m/s + CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning) if candidate not in RAM_CARS: # Newer FW versions standard on the following platforms, or flashed by a dealer onto older platforms have a higher minimum steering speed. new_eps_platform = candidate in (CAR.PACIFICA_2019_HYBRID, CAR.PACIFICA_2020, CAR.JEEP_CHEROKEE_2019) @@ -36,9 +40,12 @@ class CarInterface(CarInterfaceBase): ret.mass = 2242. + STD_CARGO_KG ret.wheelbase = 3.089 ret.steerRatio = 16.2 # Pacifica Hybrid 2017 - ret.lateralTuning.pid.kpBP, ret.lateralTuning.pid.kiBP = [[9., 20.], [9., 20.]] - ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.15, 0.30], [0.03, 0.05]] - ret.lateralTuning.pid.kf = 0.00006 + + if candidate in CHRYSLER_OLD_TUNING_BLACKLIST: + ret.lateralTuning.init('pid') + ret.lateralTuning.pid.kpBP, ret.lateralTuning.pid.kiBP = [[9., 20.], [9., 20.]] + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.15, 0.30], [0.03, 0.05]] + ret.lateralTuning.pid.kf = 0.00006 # Jeep elif candidate in (CAR.JEEP_CHEROKEE, CAR.JEEP_CHEROKEE_2019): @@ -46,9 +53,12 @@ class CarInterface(CarInterfaceBase): ret.wheelbase = 2.71 ret.steerRatio = 16.7 ret.steerActuatorDelay = 0.2 - ret.lateralTuning.pid.kpBP, ret.lateralTuning.pid.kiBP = [[9., 20.], [9., 20.]] - ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.15, 0.30], [0.03, 0.05]] - ret.lateralTuning.pid.kf = 0.00006 + + if candidate in CHRYSLER_OLD_TUNING_BLACKLIST: + ret.lateralTuning.init('pid') + ret.lateralTuning.pid.kpBP, ret.lateralTuning.pid.kiBP = [[9., 20.], [9., 20.]] + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.15, 0.30], [0.03, 0.05]] + ret.lateralTuning.pid.kf = 0.00006 # Ram elif candidate == CAR.RAM_1500: @@ -56,11 +66,10 @@ class CarInterface(CarInterfaceBase): ret.wheelbase = 3.88 ret.steerRatio = 16.3 ret.mass = 2493. + STD_CARGO_KG - CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning) ret.minSteerSpeed = 14.5 - for fw in car_fw: - if fw.ecu == 'eps' and fw.fwVersion.startswith((b"68312176", b"68273275")): - ret.minSteerSpeed = 0. + # Older EPS FW allow steer to zero + if any(fw.ecu == 'eps' and fw.fwVersion[:4] <= b"6831" for fw in car_fw): + ret.minSteerSpeed = 0. elif candidate == CAR.RAM_HD: ret.steerActuatorDelay = 0.2 @@ -100,5 +109,5 @@ class CarInterface(CarInterfaceBase): return ret - def apply(self, c): - return self.CC.update(c, self.CS) + def apply(self, c, now_nanos): + return self.CC.update(c, self.CS, now_nanos) diff --git a/selfdrive/car/chrysler/radar_interface.py b/selfdrive/car/chrysler/radar_interface.py index 348e3c3632..0ab8c10b44 100755 --- a/selfdrive/car/chrysler/radar_interface.py +++ b/selfdrive/car/chrysler/radar_interface.py @@ -45,12 +45,13 @@ def _address_to_track(address): class RadarInterface(RadarInterfaceBase): def __init__(self, CP): super().__init__(CP) + self.CP = CP self.rcp = _create_radar_can_parser(CP.carFingerprint) self.updated_messages = set() self.trigger_msg = LAST_MSG def update(self, can_strings): - if self.rcp is None: + if self.rcp is None or self.CP.radarUnavailable: return super().update(None) vls = self.rcp.update_strings(can_strings) diff --git a/selfdrive/car/chrysler/values.py b/selfdrive/car/chrysler/values.py index 7629a2f086..c6b8bcadb0 100644 --- a/selfdrive/car/chrysler/values.py +++ b/selfdrive/car/chrysler/values.py @@ -46,8 +46,12 @@ class CarControllerParams: self.STEER_DELTA_DOWN = 6 self.STEER_MAX = 261 # EPS allows more, up to 350? else: - self.STEER_DELTA_UP = 3 - self.STEER_DELTA_DOWN = 3 + if CP.carFingerprint in CHRYSLER_OLD_TUNING_BLACKLIST: + self.STEER_DELTA_UP = 3 + self.STEER_DELTA_DOWN = 3 + else: + self.STEER_DELTA_UP = 6 + self.STEER_DELTA_DOWN = 6 self.STEER_MAX = 261 # higher than this faults the EPS STEER_THRESHOLD = 120 @@ -56,6 +60,10 @@ RAM_DT = {CAR.RAM_1500, } RAM_HD = {CAR.RAM_HD, } RAM_CARS = RAM_DT | RAM_HD +# the increased steer rate hasn't been verified on these cars. +# remove from this list once it's been tested and confirmed to not fault +CHRYSLER_OLD_TUNING_BLACKLIST = {CAR.PACIFICA_2017_HYBRID, CAR.PACIFICA_2018_HYBRID, CAR.PACIFICA_2020, CAR.JEEP_CHEROKEE} + @dataclass class ChryslerCarInfo(CarInfo): package: str = "Adaptive Cruise Control (ACC)" @@ -134,7 +142,7 @@ FINGERPRINTS = { }], CAR.JEEP_CHEROKEE_2019: [{ # Jeep Grand Cherokee 2019, including most 2020 models - 55: 8, 168: 8, 179: 8, 181: 8, 256: 4, 257: 5, 258: 8, 264: 8, 268: 8, 272: 6, 273: 6, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 292: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 341: 8, 344: 8, 352: 8, 362: 8, 368: 8, 376: 3, 384: 8, 388: 4, 416: 7, 448: 6, 456: 4, 464: 8, 500: 8, 501: 8, 512: 8, 514: 8, 520: 8, 530: 8, 532: 8, 544: 8, 557: 8, 559: 8, 560: 8, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 618: 8, 624: 8, 625: 8, 632: 8, 639: 8, 640: 1, 656: 4, 658: 6, 660: 8, 671: 8, 672: 8, 676: 8, 678: 8, 680: 8, 683: 8, 684: 8, 703: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 737: 8, 738: 8, 746: 5, 752: 2, 754: 8, 760: 8, 761: 8, 764: 8, 766: 8, 773: 8, 776: 8, 779: 8, 782: 8, 783: 8, 784: 8, 785: 8, 792: 8, 799: 8, 800: 8, 804: 8, 806: 2, 808: 8, 810: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 831: 6, 832: 8, 838: 2, 840: 8, 844: 5, 847: 1, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 882: 8, 897: 8, 906: 8, 924: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 960: 4, 968: 8, 969: 4, 970: 8, 973: 8, 974: 5, 976: 8, 977: 4, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1062: 8, 1098: 8, 1100: 8, 1216: 8, 1218: 8, 1220: 8, 1223: 8, 1225: 8, 1227: 8, 1235: 8, 1242: 8, 1250: 8, 1251: 8, 1252: 8, 1254: 8, 1264: 8, 1284: 8, 1536: 8, 1537: 8, 1543: 8, 1545: 8, 1562: 8, 1568: 8, 1570: 8, 1572: 8, 1593: 8, 1856: 8, 1858: 8, 1860: 8, 1863: 8, 1865: 8, 1867: 8, 1875: 8, 1882: 8, 1890: 8, 1891: 8, 1892: 8, 1894: 8, 1896: 8, 1904: 8, 2015: 8, 2016: 8, 2017: 8, 2024: 8, 2025: 8 + 55: 8, 168: 8, 179: 8, 181: 8, 256: 4, 257: 5, 258: 8, 264: 8, 268: 8, 272: 6, 273: 6, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 292: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 341: 8, 344: 8, 352: 8, 362: 8, 368: 8, 376: 3, 384: 8, 388: 4, 416: 7, 448: 6, 456: 4, 464: 8, 500: 8, 501: 8, 512: 8, 514: 8, 520: 8, 530: 8, 532: 8, 544: 8, 557: 8, 559: 8, 560: 8, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 618: 8, 624: 8, 625: 8, 632: 8, 639: 8, 640: 1, 656: 4, 658: 6, 660: 8, 671: 8, 672: 8, 676: 8, 678: 8, 680: 8, 683: 8, 684: 8, 703: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 737: 8, 738: 8, 746: 5, 752: 2, 754: 8, 760: 8, 761: 8, 764: 8, 766: 8, 773: 8, 776: 8, 779: 8, 782: 8, 783: 8, 784: 8, 785: 8, 792: 8, 799: 8, 800: 8, 804: 8, 806: 2, 808: 8, 810: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 831: 6, 832: 8, 838: 2, 840: 8, 844: 5, 847: 1, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 874: 2, 882: 8, 897: 8, 906: 8, 924: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 960: 4, 968: 8, 969: 4, 970: 8, 973: 8, 974: 5, 976: 8, 977: 4, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1062: 8, 1098: 8, 1100: 8, 1216: 8, 1218: 8, 1220: 8, 1223: 8, 1225: 8, 1227: 8, 1235: 8, 1242: 8, 1250: 8, 1251: 8, 1252: 8, 1254: 8, 1264: 8, 1284: 8, 1536: 8, 1537: 8, 1543: 8, 1545: 8, 1562: 8, 1568: 8, 1570: 8, 1572: 8, 1593: 8, 1856: 8, 1858: 8, 1860: 8, 1863: 8, 1865: 8, 1867: 8, 1875: 8, 1882: 8, 1890: 8, 1891: 8, 1892: 8, 1894: 8, 1896: 8, 1904: 8, 2015: 8, 2016: 8, 2017: 8, 2024: 8, 2025: 8 }], } diff --git a/selfdrive/car/docs_definitions.py b/selfdrive/car/docs_definitions.py index e047b33e76..c0fb4420df 100644 --- a/selfdrive/car/docs_definitions.py +++ b/selfdrive/car/docs_definitions.py @@ -204,6 +204,13 @@ class CarInfo: if self.row[Column.STEERING_TORQUE] != Star.FULL: sentence_builder += " This car may not be able to take tight turns on its own." + # experimental mode + exp_link = "Experimental mode" + if CP.openpilotLongitudinalControl or CP.experimentalLongitudinalAvailable: + sentence_builder += f" Traffic light and stop sign handling is also available in {exp_link}." + else: + sentence_builder += f" {exp_link}, with traffic light and stop sign handling, is not currently available for this car, but may be added in a future software update." + return sentence_builder.format(car_model=f"{self.make} {self.model}", alc=alc, acc=acc) else: diff --git a/selfdrive/car/ecu_addrs.py b/selfdrive/car/ecu_addrs.py index 9f6ace2b5f..e5d550fac8 100755 --- a/selfdrive/car/ecu_addrs.py +++ b/selfdrive/car/ecu_addrs.py @@ -87,5 +87,5 @@ if __name__ == "__main__": for addr, subaddr, bus in ecu_addrs: msg = f" 0x{hex(addr)}" if subaddr is not None: - msg += f" (sub-address: 0x{hex(subaddr)})" + msg += f" (sub-address: {hex(subaddr)})" print(msg) diff --git a/selfdrive/car/ford/carcontroller.py b/selfdrive/car/ford/carcontroller.py index 8c391bb276..99072ae975 100644 --- a/selfdrive/car/ford/carcontroller.py +++ b/selfdrive/car/ford/carcontroller.py @@ -1,28 +1,14 @@ -import math from cereal import car -from common.numpy_fast import clip, interp +from common.numpy_fast import clip from opendbc.can.packer import CANPacker -from selfdrive.car.ford import fordcan -from selfdrive.car.ford.values import CANBUS, CarControllerParams +from selfdrive.car import apply_std_steer_angle_limits +from selfdrive.car.ford.fordcan import create_acc_command, create_acc_ui_msg, create_button_msg, create_lat_ctl_msg, \ + create_lat_ctl2_msg, create_lka_msg, create_lkas_ui_msg +from selfdrive.car.ford.values import CANBUS, CANFD_CARS, CarControllerParams VisualAlert = car.CarControl.HUDControl.VisualAlert -def apply_ford_steer_angle_limits(apply_angle, apply_angle_last, vEgo): - # rate limit - steer_up = apply_angle_last * apply_angle > 0. and abs(apply_angle) > abs(apply_angle_last) - rate_limit = CarControllerParams.RATE_LIMIT_UP if steer_up else CarControllerParams.RATE_LIMIT_DOWN - max_angle_diff = interp(vEgo, rate_limit.speed_points, rate_limit.max_angle_diff_points) - apply_angle = clip(apply_angle, (apply_angle_last - max_angle_diff), (apply_angle_last + max_angle_diff)) - - # absolute limit (LatCtlPath_An_Actl) - apply_path_angle = math.radians(apply_angle) / CarControllerParams.LKAS_STEER_RATIO - apply_path_angle = clip(apply_path_angle, -0.5, 0.5235) - apply_angle = math.degrees(apply_path_angle) * CarControllerParams.LKAS_STEER_RATIO - - return apply_angle - - class CarController: def __init__(self, dbc_name, CP, VM): self.CP = CP @@ -30,12 +16,12 @@ class CarController: self.packer = CANPacker(dbc_name) self.frame = 0 - self.apply_angle_last = 0 + self.apply_curvature_last = 0 self.main_on_last = False self.lkas_enabled_last = False self.steer_alert_last = False - def update(self, CC, CS): + def update(self, CC, CS, now_nanos): can_sends = [] actuators = CC.actuators @@ -46,66 +32,69 @@ class CarController: ### acc buttons ### if CC.cruiseControl.cancel: - can_sends.append(fordcan.create_button_command(self.packer, CS.buttons_stock_values, cancel=True)) - can_sends.append(fordcan.create_button_command(self.packer, CS.buttons_stock_values, cancel=True, bus=CANBUS.main)) + can_sends.append(create_button_msg(self.packer, CS.buttons_stock_values, cancel=True)) + can_sends.append(create_button_msg(self.packer, CS.buttons_stock_values, cancel=True, bus=CANBUS.main)) elif CC.cruiseControl.resume and (self.frame % CarControllerParams.BUTTONS_STEP) == 0: - can_sends.append(fordcan.create_button_command(self.packer, CS.buttons_stock_values, resume=True)) - can_sends.append(fordcan.create_button_command(self.packer, CS.buttons_stock_values, resume=True, bus=CANBUS.main)) + can_sends.append(create_button_msg(self.packer, CS.buttons_stock_values, resume=True)) + can_sends.append(create_button_msg(self.packer, CS.buttons_stock_values, resume=True, bus=CANBUS.main)) # if stock lane centering isn't off, send a button press to toggle it off # the stock system checks for steering pressed, and eventually disengages cruise control elif CS.acc_tja_status_stock_values["Tja_D_Stat"] != 0 and (self.frame % CarControllerParams.ACC_UI_STEP) == 0: - can_sends.append(fordcan.create_button_command(self.packer, CS.buttons_stock_values, tja_toggle=True)) - + can_sends.append(create_button_msg(self.packer, CS.buttons_stock_values, tja_toggle=True)) ### lateral control ### - if CC.latActive: - lca_rq = 1 - apply_angle = apply_ford_steer_angle_limits(actuators.steeringAngleDeg, self.apply_angle_last, CS.out.vEgo) - else: - lca_rq = 0 - apply_angle = 0. - # send steering commands at 20Hz if (self.frame % CarControllerParams.STEER_STEP) == 0: - # use LatCtlPath_An_Actl to actuate steering - path_angle = math.radians(apply_angle) / CarControllerParams.LKAS_STEER_RATIO - - # set slower ramp type when small steering angle change - # 0=Slow, 1=Medium, 2=Fast, 3=Immediately - steer_change = abs(CS.out.steeringAngleDeg - actuators.steeringAngleDeg) - if steer_change < 2.0: - ramp_type = 0 - elif steer_change < 4.0: - ramp_type = 1 - elif steer_change < 6.0: - ramp_type = 2 + if CC.latActive: + # apply limits to curvature and clip to signal range + apply_curvature = apply_std_steer_angle_limits(actuators.curvature, self.apply_curvature_last, CS.out.vEgo, CarControllerParams) + apply_curvature = clip(apply_curvature, -CarControllerParams.CURVATURE_MAX, CarControllerParams.CURVATURE_MAX) else: - ramp_type = 3 - precision = 1 # 0=Comfortable, 1=Precise (the stock system always uses comfortable) + apply_curvature = 0. - self.apply_angle_last = apply_angle - can_sends.append(fordcan.create_lka_command(self.packer, 0, 0)) - can_sends.append(fordcan.create_tja_command(self.packer, lca_rq, ramp_type, precision, - 0, path_angle, 0, 0)) + self.apply_curvature_last = apply_curvature + can_sends.append(create_lka_msg(self.packer)) + + if self.CP.carFingerprint in CANFD_CARS: + # TODO: extended mode + mode = 1 if CC.latActive else 0 + counter = self.frame // CarControllerParams.STEER_STEP + can_sends.append(create_lat_ctl2_msg(self.packer, mode, 0., 0., -apply_curvature, 0., counter)) + else: + can_sends.append(create_lat_ctl_msg(self.packer, CC.latActive, 0., 0., -apply_curvature, 0.)) + + ### longitudinal control ### + # send acc command at 50Hz + if self.CP.openpilotLongitudinalControl and (self.frame % CarControllerParams.ACC_CONTROL_STEP) == 0: + accel = clip(actuators.accel, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX) + + precharge_brake = accel < -0.1 + if accel > -0.5: + gas = accel + decel = False + else: + gas = -5.0 + decel = True + can_sends.append(create_acc_command(self.packer, CC.longActive, gas, accel, precharge_brake, decel)) ### ui ### send_ui = (self.main_on_last != main_on) or (self.lkas_enabled_last != CC.latActive) or (self.steer_alert_last != steer_alert) # send lkas ui command at 1Hz or if ui state changes if (self.frame % CarControllerParams.LKAS_UI_STEP) == 0 or send_ui: - can_sends.append(fordcan.create_lkas_ui_command(self.packer, main_on, CC.latActive, steer_alert, hud_control, CS.lkas_status_stock_values)) + can_sends.append(create_lkas_ui_msg(self.packer, main_on, CC.latActive, steer_alert, hud_control, CS.lkas_status_stock_values)) # send acc ui command at 20Hz or if ui state changes if (self.frame % CarControllerParams.ACC_UI_STEP) == 0 or send_ui: - can_sends.append(fordcan.create_acc_ui_command(self.packer, main_on, CC.latActive, hud_control, CS.acc_tja_status_stock_values)) + can_sends.append(create_acc_ui_msg(self.packer, main_on, CC.latActive, hud_control, CS.acc_tja_status_stock_values)) self.main_on_last = main_on self.lkas_enabled_last = CC.latActive self.steer_alert_last = steer_alert new_actuators = actuators.copy() - new_actuators.steeringAngleDeg = self.apply_angle_last + new_actuators.curvature = self.apply_curvature_last self.frame += 1 return new_actuators, can_sends diff --git a/selfdrive/car/ford/carstate.py b/selfdrive/car/ford/carstate.py index 2276b1208a..f97225e9f2 100644 --- a/selfdrive/car/ford/carstate.py +++ b/selfdrive/car/ford/carstate.py @@ -14,7 +14,7 @@ class CarState(CarStateBase): super().__init__(CP) can_define = CANDefine(DBC[CP.carFingerprint]["pt"]) if CP.transmissionType == TransmissionType.automatic: - self.shifter_values = can_define.dv["Gear_Shift_by_Wire_FD1"]["TrnGear_D_RqDrv"] + self.shifter_values = can_define.dv["Gear_Shift_by_Wire_FD1"]["TrnRng_D_RqGsm"] def update(self, cp, cp_cam): ret = car.CarState.new_message() @@ -51,7 +51,7 @@ class CarState(CarStateBase): # gear if self.CP.transmissionType == TransmissionType.automatic: - gear = self.shifter_values.get(cp.vl["Gear_Shift_by_Wire_FD1"]["TrnGear_D_RqDrv"], None) + gear = self.shifter_values.get(cp.vl["Gear_Shift_by_Wire_FD1"]["TrnRng_D_RqGsm"]) ret.gearShifter = self.parse_gear_shifter(gear) elif self.CP.transmissionType == TransmissionType.manual: ret.clutchPressed = cp.vl["Engine_Clutch_Data"]["CluPdlPos_Pc_Meas"] > 0 @@ -118,7 +118,7 @@ class CarState(CarStateBase): ("DrStatRl_B_Actl", "BodyInfo_3_FD1"), # BCM Door open, rear left ("DrStatRr_B_Actl", "BodyInfo_3_FD1"), # BCM Door open, rear right ("FirstRowBuckleDriver", "RCMStatusMessage2_FD1"), # RCM Seatbelt status, driver - ("HeadLghtHiFlash_D_Stat", "Steering_Data_FD1"), # SCCM Passthru the remaining buttons + ("HeadLghtHiFlash_D_Stat", "Steering_Data_FD1"), # SCCM Passthrough the remaining buttons ("WiprFront_D_Stat", "Steering_Data_FD1"), ("LghtAmb_D_Sns", "Steering_Data_FD1"), ("AccButtnGapDecPress", "Steering_Data_FD1"), @@ -171,7 +171,7 @@ class CarState(CarStateBase): if CP.transmissionType == TransmissionType.automatic: signals += [ - ("TrnGear_D_RqDrv", "Gear_Shift_by_Wire_FD1"), # GWM transmission gear position + ("TrnRng_D_RqGsm", "Gear_Shift_by_Wire_FD1"), # GWM transmission gear position ] checks += [ ("Gear_Shift_by_Wire_FD1", 10), diff --git a/selfdrive/car/ford/fordcan.py b/selfdrive/car/ford/fordcan.py index 373ce096c6..594d50f59f 100644 --- a/selfdrive/car/ford/fordcan.py +++ b/selfdrive/car/ford/fordcan.py @@ -4,64 +4,120 @@ from selfdrive.car.ford.values import CANBUS HUDControl = car.CarControl.HUDControl -def create_lka_command(packer, angle_deg: float, curvature: float): +def calculate_lat_ctl2_checksum(mode: int, counter: int, dat: bytearray): + checksum = mode + counter + checksum += dat[2] + ((dat[3] & 0xE0) >> 5) # curvature + checksum += dat[6] + ((dat[7] & 0xE0) >> 5) # curvature rate + checksum += (dat[3] & 0x1F) + ((dat[4] & 0xFC) >> 2) # path angle + checksum += (dat[4] & 0x3) + dat[5] # path offset + return 0xFF - (checksum & 0xFF) + + +def create_lka_msg(packer): """ - Creates a CAN message for the Ford LKAS Command. + Creates an empty CAN message for the Ford LKA Command. This command can apply "Lane Keeping Aid" manoeuvres, which are subject to the PSCM lockout. Frequency is 20Hz. """ - values = { - "LkaDrvOvrrd_D_Rq": 0, # driver override level? [0|3] - "LkaActvStats_D2_Req": 0, # action [0|7] - "LaRefAng_No_Req": angle_deg, # angle [-102.4|102.3] degrees - "LaRampType_B_Req": 0, # Ramp speed: 0=Smooth, 1=Quick - "LaCurvature_No_Calc": curvature, # curvature [-0.01024|0.01023] 1/meter - "LdwActvStats_D_Req": 0, # LDW status [0|7] - "LdwActvIntns_D_Req": 0, # LDW intensity [0|3], shake alert strength - } - return packer.make_can_msg("Lane_Assist_Data1", CANBUS.main, values) + return packer.make_can_msg("Lane_Assist_Data1", CANBUS.main, {}) -def create_tja_command(packer, lca_rq: int, ramp_type: int, precision: int, path_offset: float, path_angle: float, curvature_rate: float, curvature: float): +def create_lat_ctl_msg(packer, lat_active: bool, path_offset: float, path_angle: float, curvature: float, + curvature_rate: float): """ Creates a CAN message for the Ford TJA/LCA Command. - This command can apply "Lane Centering" manoeuvres: continuous lane centering for traffic jam - assist and highway driving. It is not subject to the PSCM lockout. + This command can apply "Lane Centering" manoeuvres: continuous lane centering for traffic jam assist and highway + driving. It is not subject to the PSCM lockout. - Ford lane centering command uses a third order polynomial to describe the road centerline. The - polynomial is defined by the following coefficients: - c0: lateral offset between the vehicle and the centerline - c1: heading angle between the vehicle and the centerline - c2: curvature of the centerline + Ford lane centering command uses a third order polynomial to describe the road centerline. The polynomial is defined + by the following coefficients: + c0: lateral offset between the vehicle and the centerline (positive is right) + c1: heading angle between the vehicle and the centerline (positive is right) + c2: curvature of the centerline (positive is left) c3: rate of change of curvature of the centerline - As the PSCM combines this information with other sensor data, such as the vehicle's yaw rate and - speed, the steering angle cannot be easily controlled. + As the PSCM combines this information with other sensor data, such as the vehicle's yaw rate and speed, the steering + angle cannot be easily controlled. - The PSCM should be configured to accept TJA/LCA commands before these commands will be processed. - This can be done using tools such as Forscan. + The PSCM should be configured to accept TJA/LCA commands before these commands will be processed. This can be done + using tools such as Forscan. Frequency is 20Hz. """ values = { - "LatCtlRng_L_Max": 0, # Unknown [0|126] meter - "HandsOffCnfm_B_Rq": 0, # Unknown: 0=Inactive, 1=Active [0|1] - "LatCtl_D_Rq": lca_rq, # Mode: 0=None, 1=ContinuousPathFollowing, 2=InterventionLeft, 3=InterventionRight, 4-7=NotUsed [0|7] - "LatCtlRampType_D_Rq": ramp_type, # Ramp speed: 0=Slow, 1=Medium, 2=Fast, 3=Immediate [0|3] - "LatCtlPrecision_D_Rq": precision, # Precision: 0=Comfortable, 1=Precise, 2/3=NotUsed [0|3] - "LatCtlPathOffst_L_Actl": path_offset, # Path offset [-5.12|5.11] meter - "LatCtlPath_An_Actl": path_angle, # Path angle [-0.5|0.5235] radians - "LatCtlCurv_NoRate_Actl": curvature_rate, # Curvature rate [-0.001024|0.00102375] 1/meter^2 - "LatCtlCurv_No_Actl": curvature, # Curvature [-0.02|0.02094] 1/meter + "LatCtlRng_L_Max": 0, # Unknown [0|126] meter + "HandsOffCnfm_B_Rq": 0, # Unknown: 0=Inactive, 1=Active [0|1] + "LatCtl_D_Rq": 1 if lat_active else 0, # Mode: 0=None, 1=ContinuousPathFollowing, 2=InterventionLeft, + # 3=InterventionRight, 4-7=NotUsed [0|7] + "LatCtlRampType_D_Rq": 0, # Ramp speed: 0=Slow, 1=Medium, 2=Fast, 3=Immediate [0|3] + # Makes no difference with curvature control + "LatCtlPrecision_D_Rq": 1, # Precision: 0=Comfortable, 1=Precise, 2/3=NotUsed [0|3] + # The stock system always uses comfortable + "LatCtlPathOffst_L_Actl": path_offset, # Path offset [-5.12|5.11] meter + "LatCtlPath_An_Actl": path_angle, # Path angle [-0.5|0.5235] radians + "LatCtlCurv_NoRate_Actl": curvature_rate, # Curvature rate [-0.001024|0.00102375] 1/meter^2 + "LatCtlCurv_No_Actl": curvature, # Curvature [-0.02|0.02094] 1/meter } return packer.make_can_msg("LateralMotionControl", CANBUS.main, values) -def create_lkas_ui_command(packer, main_on: bool, enabled: bool, steer_alert: bool, hud_control, stock_values: dict): +def create_lat_ctl2_msg(packer, mode: int, path_offset: float, path_angle: float, curvature: float, + curvature_rate: float, counter: int): + """ + Create a CAN message for the new Ford Lane Centering command. + + This message is used on the CAN FD platform and replaces the old LateralMotionControl message. It is similar but has + additional signals for a counter and checksum. + + Frequency is 20Hz. + """ + + values = { + "LatCtl_D2_Rq": mode, # Mode: 0=None, 1=PathFollowingLimitedMode, 2=PathFollowingExtendedMode, + # 3=SafeRampOut, 4-7=NotUsed [0|7] + "LatCtlRampType_D_Rq": 0, # 0=Slow, 1=Medium, 2=Fast, 3=Immediate [0|3] + "LatCtlPrecision_D_Rq": 1, # 0=Comfortable, 1=Precise, 2/3=NotUsed [0|3] + "LatCtlPathOffst_L_Actl": path_offset, # [-5.12|5.11] meter + "LatCtlPath_An_Actl": path_angle, # [-0.5|0.5235] radians + "LatCtlCurv_No_Actl": curvature, # [-0.02|0.02094] 1/meter + "LatCtlCrv_NoRate2_Actl": curvature_rate, # [-0.001024|0.001023] 1/meter^2 + "HandsOffCnfm_B_Rq": 0, # 0=Inactive, 1=Active [0|1] + "LatCtlPath_No_Cnt": counter, # [0|15] + "LatCtlPath_No_Cs": 0, # [0|255] + } + + # calculate checksum + dat = packer.make_can_msg("LateralMotionControl2", CANBUS.main, values)[2] + values["LatCtlPath_No_Cs"] = calculate_lat_ctl2_checksum(mode, counter, dat) + + return packer.make_can_msg("LateralMotionControl2", CANBUS.main, values) + + +def create_acc_command(packer, long_active: bool, gas: float, accel: float, precharge_brake: bool, decel: bool): + """ + Creates a CAN message for the Ford ACC Command. + + This command can be used to enable ACC, to set the ACC gas/brake/decel values + and to disable ACC. + + Frequency is 50Hz. + """ + + values = { + "AccBrkTot_A_Rq": accel, # Brake total accel request: [-20|11.9449] m/s^2 + "Cmbb_B_Enbl": 1 if long_active else 0, # Enabled: 0=No, 1=Yes + "AccPrpl_A_Rq": gas, # Acceleration request: [-5|5.23] m/s^2 + "AccBrkPrchg_B_Rq": 1 if precharge_brake else 0, # Pre-charge brake request: 0=No, 1=Yes + "AccBrkDecel_B_Rq": 1 if decel else 0, # Deceleration request: 0=Inactive, 1=Active + } + return packer.make_can_msg("ACCDATA", CANBUS.main, values) + + +def create_lkas_ui_msg(packer, main_on: bool, enabled: bool, steer_alert: bool, hud_control, stock_values: dict): """ Creates a CAN message for the Ford IPC IPMA/LKAS status. @@ -107,16 +163,15 @@ def create_lkas_ui_command(packer, main_on: bool, enabled: bool, steer_alert: bo values = { **stock_values, - "LaActvStats_D_Dsply": lines, # LKAS status (lines) [0|31] - "LaHandsOff_D_Dsply": hands_on_wheel_dsply, # 0=HandsOn, 1=Level1 (w/o chime), 2=Level2 (w/ chime), 3=Suppressed + "LaActvStats_D_Dsply": lines, # LKAS status (lines) [0|31] + "LaHandsOff_D_Dsply": hands_on_wheel_dsply, # 0=HandsOn, 1=Level1 (w/o chime), 2=Level2 (w/ chime), 3=Suppressed } return packer.make_can_msg("IPMA_Data", CANBUS.main, values) -def create_acc_ui_command(packer, main_on: bool, enabled: bool, hud_control, stock_values: dict): +def create_acc_ui_msg(packer, main_on: bool, enabled: bool, hud_control, stock_values: dict): """ - Creates a CAN message for the Ford IPC adaptive cruise, forward collision warning and traffic jam - assist status. + Creates a CAN message for the Ford IPC adaptive cruise, forward collision warning and traffic jam assist status. Stock functionality is maintained by passing through unmodified signals. @@ -148,7 +203,8 @@ def create_acc_ui_command(packer, main_on: bool, enabled: bool, hud_control, sto return packer.make_can_msg("ACCDATA_3", CANBUS.main, values) -def create_button_command(packer, stock_values: dict, cancel = False, resume = False, tja_toggle = False, bus: int = CANBUS.camera): +def create_button_msg(packer, stock_values: dict, cancel=False, resume=False, tja_toggle=False, + bus: int = CANBUS.camera): """ Creates a CAN message for the Ford SCCM buttons/switches. diff --git a/selfdrive/car/ford/interface.py b/selfdrive/car/ford/interface.py index f3d77bc05a..9e1366618c 100644 --- a/selfdrive/car/ford/interface.py +++ b/selfdrive/car/ford/interface.py @@ -2,39 +2,52 @@ from cereal import car from common.conversions import Conversions as CV from selfdrive.car import STD_CARGO_KG, get_safety_config -from selfdrive.car.ford.values import CAR, Ecu, TransmissionType, GearShifter +from selfdrive.car.ford.values import CAR, Ecu from selfdrive.car.interfaces import CarInterfaceBase -CarParams = car.CarParams +TransmissionType = car.CarParams.TransmissionType +GearShifter = car.CarState.GearShifter class CarInterface(CarInterfaceBase): @staticmethod def _get_params(ret, candidate, fingerprint, car_fw, experimental_long): ret.carName = "ford" + ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.ford)] + + # These cars are dashcam only until the port is finished ret.dashcamOnly = True - ret.safetyConfigs = [get_safety_config(CarParams.SafetyModel.ford)] - # Angle-based steering - ret.steerControlType = CarParams.SteerControlType.angle - ret.steerActuatorDelay = 0.4 + ret.radarUnavailable = True + ret.steerControlType = car.CarParams.SteerControlType.angle + ret.steerActuatorDelay = 0.2 ret.steerLimitTimer = 1.0 - if candidate == CAR.ESCAPE_MK4: + if candidate == CAR.BRONCO_SPORT_MK1: + ret.wheelbase = 2.67 + ret.steerRatio = 17.7 + ret.mass = 1625 + STD_CARGO_KG + + elif candidate == CAR.ESCAPE_MK4: ret.wheelbase = 2.71 - ret.steerRatio = 14.3 # Copied from Focus + ret.steerRatio = 16.7 ret.mass = 1750 + STD_CARGO_KG elif candidate == CAR.EXPLORER_MK6: ret.wheelbase = 3.025 - ret.steerRatio = 16.8 # learned + ret.steerRatio = 16.8 ret.mass = 2050 + STD_CARGO_KG elif candidate == CAR.FOCUS_MK4: ret.wheelbase = 2.7 - ret.steerRatio = 13.8 # learned + ret.steerRatio = 15.0 ret.mass = 1350 + STD_CARGO_KG + elif candidate == CAR.MAVERICK_MK1: + ret.wheelbase = 3.076 + ret.steerRatio = 17.0 + ret.mass = 1650 + STD_CARGO_KG + else: raise ValueError(f"Unsupported car: {candidate}") @@ -65,5 +78,5 @@ class CarInterface(CarInterfaceBase): return ret - def apply(self, c): - return self.CC.update(c, self.CS) + def apply(self, c, now_nanos): + return self.CC.update(c, self.CS, now_nanos) diff --git a/selfdrive/car/ford/radar_interface.py b/selfdrive/car/ford/radar_interface.py index c942703002..ee4efb311d 100644 --- a/selfdrive/car/ford/radar_interface.py +++ b/selfdrive/car/ford/radar_interface.py @@ -47,7 +47,7 @@ class RadarInterface(RadarInterfaceBase): self.updated_messages = set() self.track_id = 0 self.radar = DBC[CP.carFingerprint]['radar'] - if self.radar is None: + if self.radar is None or CP.radarUnavailable: self.rcp = None elif self.radar == RADAR.DELPHI_ESR: self.rcp = _create_delphi_esr_radar_can_parser() diff --git a/selfdrive/car/ford/values.py b/selfdrive/car/ford/values.py index 0e6ef464b3..526b74b16c 100644 --- a/selfdrive/car/ford/values.py +++ b/selfdrive/car/ford/values.py @@ -1,23 +1,21 @@ -from collections import defaultdict, namedtuple +from collections import defaultdict from dataclasses import dataclass from enum import Enum -from typing import Dict, List, Union +from typing import Dict, List, Set, Union from cereal import car -from selfdrive.car import dbc_dict +from selfdrive.car import AngleRateLimit, dbc_dict from selfdrive.car.docs_definitions import CarInfo, Harness from selfdrive.car.fw_query_definitions import FwQueryConfig, Request, StdQueries Ecu = car.CarParams.Ecu -TransmissionType = car.CarParams.TransmissionType -GearShifter = car.CarState.GearShifter - -AngleRateLimit = namedtuple('AngleRateLimit', ['speed_points', 'max_angle_diff_points']) class CarControllerParams: # Messages: Lane_Assist_Data1, LateralMotionControl STEER_STEP = 5 + # Message: ACCDATA + ACC_CONTROL_STEP = 2 # Message: IPMA_Data LKAS_UI_STEP = 100 # Message: ACCDATA_3 @@ -25,12 +23,16 @@ class CarControllerParams: # Message: Steering_Data_FD1, but send twice as fast BUTTONS_STEP = 10 / 2 - LKAS_STEER_RATIO = 2.75 # Approximate ratio between LatCtlPath_An_Actl and steering angle in radians - # TODO: remove this once we understand how the EPS calculates the steering angle better - STEER_DRIVER_ALLOWANCE = 0.8 # Driver intervention threshold in Nm + CURVATURE_MAX = 0.02 # Max curvature for steering command, m^-1 + STEER_DRIVER_ALLOWANCE = 0.8 # Driver intervention threshold, Nm + + # Curvature rate limits + # TODO: unify field names used by curvature and angle control cars + ANGLE_RATE_LIMIT_UP = AngleRateLimit(speed_bp=[5, 15, 25], angle_v=[0.005, 0.00056, 0.0002]) + ANGLE_RATE_LIMIT_DOWN = AngleRateLimit(speed_bp=[5, 15, 25], angle_v=[0.008, 0.00089, 0.00032]) - RATE_LIMIT_UP = AngleRateLimit(speed_points=[0., 5., 15.], max_angle_diff_points=[5., .8, .15]) - RATE_LIMIT_DOWN = AngleRateLimit(speed_points=[0., 5., 15.], max_angle_diff_points=[5., 3.5, 0.4]) + ACCEL_MAX = 2.0 # m/s^s max acceleration + ACCEL_MIN = -3.5 # m/s^s max deceleration def __init__(self, CP): pass @@ -43,9 +45,14 @@ class CANBUS: class CAR: + BRONCO_SPORT_MK1 = "FORD BRONCO SPORT 1ST GEN" ESCAPE_MK4 = "FORD ESCAPE 4TH GEN" EXPLORER_MK6 = "FORD EXPLORER 6TH GEN" FOCUS_MK4 = "FORD FOCUS 4TH GEN" + MAVERICK_MK1 = "FORD MAVERICK 1ST GEN" + + +CANFD_CARS: Set[str] = set() class RADAR: @@ -63,12 +70,17 @@ class FordCarInfo(CarInfo): CAR_INFO: Dict[str, Union[CarInfo, List[CarInfo]]] = { + CAR.BRONCO_SPORT_MK1: FordCarInfo("Ford Bronco Sport 2021-22"), CAR.ESCAPE_MK4: [ FordCarInfo("Ford Escape 2020-21"), FordCarInfo("Ford Kuga 2020-21", "Driver Assistance Pack"), ], - CAR.EXPLORER_MK6: FordCarInfo("Ford Explorer 2020-22"), + CAR.EXPLORER_MK6: [ + FordCarInfo("Ford Explorer 2020-22"), + FordCarInfo("Lincoln Aviator 2021", "Co-Pilot360 Plus"), + ], CAR.FOCUS_MK4: FordCarInfo("Ford Focus EU 2019", "Driver Assistance Pack"), + CAR.MAVERICK_MK1: FordCarInfo("Ford Maverick 2022", "Co-Pilot360 Assist"), } FW_QUERY_CONFIG = FwQueryConfig( @@ -88,6 +100,30 @@ FW_QUERY_CONFIG = FwQueryConfig( ) FW_VERSIONS = { + CAR.BRONCO_SPORT_MK1: { + (Ecu.eps, 0x730, None): [ + b'LX6C-14D003-AH\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'LX6C-14D003-AK\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.abs, 0x760, None): [ + b'LX6C-2D053-RD\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'LX6C-2D053-RE\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.fwdRadar, 0x764, None): [ + b'LB5T-14D049-AB\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.fwdCamera, 0x706, None): [ + b'M1PT-14F397-AC\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.engine, 0x7E0, None): [ + b'M1PA-14C204-GF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'N1PA-14C204-AC\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.shiftByWire, 0x732, None): [ + b'LX6P-14G395-AD\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'PZ1P-14G395-AB\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + }, CAR.ESCAPE_MK4: { (Ecu.eps, 0x730, None): [ b'LX6C-14D003-AF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', @@ -132,13 +168,16 @@ FW_VERSIONS = { (Ecu.fwdCamera, 0x706, None): [ b'LB5T-14F397-AE\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'LB5T-14F397-AF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'LC5T-14F397-AH\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', ], (Ecu.engine, 0x7E0, None): [ b'LB5A-14C204-EAC\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'MB5A-14C204-MD\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'MB5A-14C204-RC\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'NB5A-14C204-HB\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', ], (Ecu.shiftByWire, 0x732, None): [ + b'L1MP-14C561-AB\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'L1MP-14G395-AD\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'L1MP-14G395-AE\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'L1MP-14G395-JB\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', @@ -163,4 +202,26 @@ FW_VERSIONS = { (Ecu.shiftByWire, 0x732, None): [ ], }, + CAR.MAVERICK_MK1: { + (Ecu.eps, 0x730, None): [ + b'NZ6C-14D003-AL\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.abs, 0x760, None): [ + b'NZ6C-2D053-AG\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.fwdRadar, 0x764, None): [ + b'NZ6T-14D049-AA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.fwdCamera, 0x706, None): [ + b'NZ6T-14F397-AC\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.engine, 0x7E0, None): [ + b'NZ6A-14C204-AAA\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'NZ6A-14C204-PA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'NZ6A-14C204-ZA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.shiftByWire, 0x732, None): [ + b'NZ6P-14G395-AD\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + }, } diff --git a/selfdrive/car/fw_versions.py b/selfdrive/car/fw_versions.py index 7ac5f80f32..acf74da47b 100755 --- a/selfdrive/car/fw_versions.py +++ b/selfdrive/car/fw_versions.py @@ -150,12 +150,16 @@ def match_fw_to_car(fw_versions, allow_exact=True, allow_fuzzy=True): return True, set() -def get_present_ecus(logcan, sendcan): +def get_present_ecus(logcan, sendcan, num_pandas=1) -> Set[Tuple[int, Optional[int], int]]: queries = list() parallel_queries = list() responses = set() for brand, r in REQUESTS: + # Skip query if no panda available + if r.bus > num_pandas * 4 - 1: + continue + for brand_versions in VERSIONS[brand].values(): for ecu_type, addr, sub_addr in brand_versions: # Only query ecus in whitelist if whitelist is not empty @@ -175,7 +179,7 @@ def get_present_ecus(logcan, sendcan): queries.insert(0, parallel_queries) - ecu_responses: Set[Tuple[int, Optional[int], int]] = set() + ecu_responses = set() for query in queries: ecu_responses.update(get_ecu_addrs(logcan, sendcan, set(query), responses, timeout=0.1)) return ecu_responses diff --git a/selfdrive/car/gm/carcontroller.py b/selfdrive/car/gm/carcontroller.py index b4a79d10a6..73085d30b0 100644 --- a/selfdrive/car/gm/carcontroller.py +++ b/selfdrive/car/gm/carcontroller.py @@ -13,6 +13,8 @@ LongCtrlState = car.CarControl.Actuators.LongControlState # Camera cancels up to 0.1s after brake is pressed, ECM allows 0.5s CAMERA_CANCEL_DELAY_FRAMES = 10 +# Enforce a minimum interval between steering messages to avoid a fault +MIN_STEER_MSG_INTERVAL_MS = 15 class CarController: @@ -37,7 +39,7 @@ class CarController: self.packer_obj = CANPacker(DBC[self.CP.carFingerprint]['radar']) self.packer_ch = CANPacker(DBC[self.CP.carFingerprint]['chassis']) - def update(self, CC, CS): + def update(self, CC, CS, now_nanos): actuators = CC.actuators hud_control = CC.hudControl hud_alert = hud_control.visualAlert @@ -49,21 +51,28 @@ class CarController: can_sends = [] # Steering (Active: 50Hz, inactive: 10Hz) - # Attempt to sync with camera on startup at 50Hz, first few msgs are blocked - init_lka_counter = not self.sent_lka_steering_cmd and self.CP.networkLocation == NetworkLocation.fwdCamera - steer_step = self.params.INACTIVE_STEER_STEP - if CC.latActive or init_lka_counter: - steer_step = self.params.STEER_STEP - - # Avoid GM EPS faults when transmitting messages too close together: skip this transmit if we just received the - # next Panda loopback confirmation in the current CS frame. + steer_step = self.params.STEER_STEP if CC.latActive else self.params.INACTIVE_STEER_STEP + + if self.CP.networkLocation == NetworkLocation.fwdCamera: + # Also send at 50Hz: + # - on startup, first few msgs are blocked + # - until we're in sync with camera so counters align when relay closes, preventing a fault. + # openpilot can subtly drift, so this is activated throughout a drive to stay synced + out_of_sync = self.lka_steering_cmd_counter % 4 != (CS.cam_lka_steering_cmd_counter + 1) % 4 + if not self.sent_lka_steering_cmd or out_of_sync: + steer_step = self.params.STEER_STEP + if CS.loopback_lka_steering_cmd_updated: self.lka_steering_cmd_counter += 1 self.sent_lka_steering_cmd = True - elif (self.frame - self.last_steer_frame) >= steer_step: + + # Avoid GM EPS faults when transmitting messages too close together: skip this transmit if we + # received the ASCMLKASteeringCmd loopback confirmation too recently + last_lka_steer_msg_ms = (now_nanos - CS.loopback_lka_steering_cmd_ts_nanos) * 1e-6 + if (self.frame - self.last_steer_frame) >= steer_step and last_lka_steer_msg_ms > MIN_STEER_MSG_INTERVAL_MS: # Initialize ASCMLKASteeringCmd counter using the camera until we get a msg on the bus - if init_lka_counter: - self.lka_steering_cmd_counter = CS.camera_lka_steering_cmd_counter + 1 + if not self.sent_lka_steering_cmd: + self.lka_steering_cmd_counter = CS.pt_lka_steering_cmd_counter + 1 if CC.latActive: new_steer = int(round(actuators.steer * self.params.STEER_MAX)) @@ -109,7 +118,7 @@ class CarController: # Radar needs to know current speed and yaw rate (50hz), # and that ADAS is alive (10hz) - if not self.CP.radarOffCan: + if not self.CP.radarUnavailable: tt = self.frame * DT_CTRL time_and_headlights_step = 10 if self.frame % time_and_headlights_step == 0: diff --git a/selfdrive/car/gm/carstate.py b/selfdrive/car/gm/carstate.py index de0fd2eed6..3c7d35f2dc 100644 --- a/selfdrive/car/gm/carstate.py +++ b/selfdrive/car/gm/carstate.py @@ -17,8 +17,13 @@ class CarState(CarStateBase): super().__init__(CP) can_define = CANDefine(DBC[CP.carFingerprint]["pt"]) self.shifter_values = can_define.dv["ECMPRDNL2"]["PRNDL2"] + self.cluster_speed_hyst_gap = CV.KPH_TO_MS / 2. + self.cluster_min_speed = CV.KPH_TO_MS / 2. + self.loopback_lka_steering_cmd_updated = False - self.camera_lka_steering_cmd_counter = 0 + self.loopback_lka_steering_cmd_ts_nanos = 0 + self.pt_lka_steering_cmd_counter = 0 + self.cam_lka_steering_cmd_counter = 0 self.buttons_counter = 0 def update(self, pt_cp, cam_cp, loopback_cp): @@ -32,8 +37,10 @@ class CarState(CarStateBase): # Variables used for avoiding LKAS faults self.loopback_lka_steering_cmd_updated = len(loopback_cp.vl_all["ASCMLKASteeringCmd"]["RollingCounter"]) > 0 + self.loopback_lka_steering_cmd_ts_nanos = loopback_cp.ts_nanos["ASCMLKASteeringCmd"]["RollingCounter"] if self.CP.networkLocation == NetworkLocation.fwdCamera: - self.camera_lka_steering_cmd_counter = cam_cp.vl["ASCMLKASteeringCmd"]["RollingCounter"] + self.pt_lka_steering_cmd_counter = pt_cp.vl["ASCMLKASteeringCmd"]["RollingCounter"] + self.cam_lka_steering_cmd_counter = cam_cp.vl["ASCMLKASteeringCmd"]["RollingCounter"] ret.wheelSpeeds = self.get_wheel_speeds( pt_cp.vl["EBCMWheelSpdFront"]["FLWheelSpd"], @@ -93,7 +100,8 @@ class CarState(CarStateBase): ret.parkingBrake = pt_cp.vl["VehicleIgnitionAlt"]["ParkBrake"] == 1 ret.cruiseState.available = pt_cp.vl["ECMEngineStatus"]["CruiseMainOn"] != 0 ret.espDisabled = pt_cp.vl["ESPStatus"]["TractionControlOn"] != 1 - ret.accFaulted = pt_cp.vl["AcceleratorPedal2"]["CruiseState"] == AccState.FAULTED + ret.accFaulted = (pt_cp.vl["AcceleratorPedal2"]["CruiseState"] == AccState.FAULTED or + pt_cp.vl["EBCMFrictionBrakeStatus"]["FrictionBrakeUnavailable"] == 1) ret.cruiseState.enabled = pt_cp.vl["AcceleratorPedal2"]["CruiseState"] != AccState.OFF ret.cruiseState.standstill = pt_cp.vl["AcceleratorPedal2"]["CruiseState"] == AccState.STANDSTILL @@ -148,6 +156,7 @@ class CarState(CarStateBase): ("RLWheelSpd", "EBCMWheelSpdRear"), ("RRWheelSpd", "EBCMWheelSpdRear"), ("MovingBackward", "EBCMWheelSpdRear"), + ("FrictionBrakeUnavailable", "EBCMFrictionBrakeStatus"), ("PRNDL2", "ECMPRDNL2"), ("ManualMode", "ECMPRDNL2"), ("LKADriverAppldTrq", "PSCMStatus"), @@ -173,6 +182,7 @@ class CarState(CarStateBase): ("VehicleIgnitionAlt", 10), ("EBCMWheelSpdFront", 20), ("EBCMWheelSpdRear", 20), + ("EBCMFrictionBrakeStatus", 20), ("AcceleratorPedal2", 33), ("ASCMSteeringButton", 33), ("ECMEngineStatus", 100), @@ -180,6 +190,15 @@ class CarState(CarStateBase): ("ECMAcceleratorPos", 80), ] + # Used to read back last counter sent to PT by camera + if CP.networkLocation == NetworkLocation.fwdCamera: + signals += [ + ("RollingCounter", "ASCMLKASteeringCmd"), + ] + checks += [ + ("ASCMLKASteeringCmd", 0), + ] + if CP.transmissionType == TransmissionType.direct: signals.append(("RegenPaddle", "EBCMRegenPaddle")) checks.append(("EBCMRegenPaddle", 50)) diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py index 856dcbaae5..8bfc067b48 100755 --- a/selfdrive/car/gm/interface.py +++ b/selfdrive/car/gm/interface.py @@ -63,10 +63,11 @@ class CarInterface(CarInterfaceBase): if candidate in CAMERA_ACC_CAR: ret.experimentalLongitudinalAvailable = True ret.networkLocation = NetworkLocation.fwdCamera - ret.radarOffCan = True # no radar + ret.radarUnavailable = True # no radar ret.pcmCruise = True ret.safetyConfigs[0].safetyParam |= Panda.FLAG_GM_HW_CAM ret.minEnableSpeed = 5 * CV.KPH_TO_MS + ret.minSteerSpeed = 10 * CV.KPH_TO_MS # Tuning for experimental long ret.longitudinalTuning.kpV = [2.0, 1.5] @@ -85,10 +86,11 @@ class CarInterface(CarInterfaceBase): else: # ASCM, OBD-II harness ret.openpilotLongitudinalControl = True ret.networkLocation = NetworkLocation.gateway - ret.radarOffCan = False + ret.radarUnavailable = False ret.pcmCruise = False # stock non-adaptive cruise control is kept off # supports stop and go, but initial engage must (conservatively) be above 18mph ret.minEnableSpeed = 18 * CV.MPH_TO_MS + ret.minSteerSpeed = 7 * CV.MPH_TO_MS # Tuning ret.longitudinalTuning.kpV = [2.4, 1.5] @@ -100,8 +102,6 @@ class CarInterface(CarInterfaceBase): ret.dashcamOnly = candidate in {CAR.CADILLAC_ATS, CAR.HOLDEN_ASTRA, CAR.MALIBU, CAR.BUICK_REGAL, CAR.EQUINOX} # Start with a baseline tuning for all GM vehicles. Override tuning as needed in each model section below. - # Some GMs need some tolerance above 10 kph to avoid a fault - ret.minSteerSpeed = 10.1 * CV.KPH_TO_MS ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2], [0.00]] ret.lateralTuning.pid.kf = 0.00004 # full torque for 20 deg at 80mph means 0.00007818594 @@ -236,5 +236,5 @@ class CarInterface(CarInterfaceBase): return ret - def apply(self, c): - return self.CC.update(c, self.CS) + def apply(self, c, now_nanos): + return self.CC.update(c, self.CS, now_nanos) diff --git a/selfdrive/car/gm/radar_interface.py b/selfdrive/car/gm/radar_interface.py index 5cb211812d..b86a85f915 100755 --- a/selfdrive/car/gm/radar_interface.py +++ b/selfdrive/car/gm/radar_interface.py @@ -36,7 +36,7 @@ class RadarInterface(RadarInterfaceBase): def __init__(self, CP): super().__init__(CP) - self.rcp = None if CP.radarOffCan else create_radar_can_parser(CP.carFingerprint) + self.rcp = None if CP.radarUnavailable else create_radar_can_parser(CP.carFingerprint) self.trigger_msg = LAST_RADAR_MSG self.updated_messages = set() diff --git a/selfdrive/car/gm/values.py b/selfdrive/car/gm/values.py index e633ec5f62..7628d8420b 100644 --- a/selfdrive/car/gm/values.py +++ b/selfdrive/car/gm/values.py @@ -11,10 +11,10 @@ Ecu = car.CarParams.Ecu class CarControllerParams: STEER_MAX = 300 # GM limit is 3Nm. Used by carcontroller to generate LKA output - STEER_STEP = 2 # Active control frames per command (50hz) + STEER_STEP = 3 # Active control frames per command (~33hz) INACTIVE_STEER_STEP = 10 # Inactive control frames per command (10hz) - STEER_DELTA_UP = 7 # Delta rates require review due to observed EPS weakness - STEER_DELTA_DOWN = 17 + STEER_DELTA_UP = 10 # Delta rates require review due to observed EPS weakness + STEER_DELTA_DOWN = 25 STEER_DRIVER_ALLOWANCE = 50 STEER_DRIVER_MULTIPLIER = 4 STEER_DRIVER_FACTOR = 100 diff --git a/selfdrive/car/honda/carcontroller.py b/selfdrive/car/honda/carcontroller.py index ab944a30aa..4dc1dc8131 100644 --- a/selfdrive/car/honda/carcontroller.py +++ b/selfdrive/car/honda/carcontroller.py @@ -124,7 +124,7 @@ class CarController: self.brake = 0.0 self.last_steer = 0.0 - def update(self, CC, CS): + def update(self, CC, CS, now_nanos): actuators = CC.actuators hud_control = CC.hudControl hud_v_cruise = hud_control.setSpeed * CV.MS_TO_KPH if hud_control.speedVisible else 255 diff --git a/selfdrive/car/honda/carstate.py b/selfdrive/car/honda/carstate.py index a37667fd3a..16880d1b1f 100644 --- a/selfdrive/car/honda/carstate.py +++ b/selfdrive/car/honda/carstate.py @@ -274,7 +274,7 @@ 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.PASSPORT, CAR.RIDGELINE): + if self.CP.carFingerprint in (CAR.PILOT, CAR.RIDGELINE): if ret.brake > 0.1: ret.brakePressed = True diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py index 990238ae5d..66c0ce4275 100755 --- a/selfdrive/car/honda/interface.py +++ b/selfdrive/car/honda/interface.py @@ -34,7 +34,7 @@ class CarInterface(CarInterfaceBase): if candidate in HONDA_BOSCH: ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.hondaBosch)] - ret.radarOffCan = True + ret.radarUnavailable = True if candidate not in HONDA_BOSCH_RADARLESS: # Disable the radar and let openpilot control longitudinal @@ -231,11 +231,11 @@ class CarInterface(CarInterfaceBase): else: ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end - elif candidate in (CAR.PILOT, CAR.PASSPORT): - ret.mass = 4204. * CV.LB_TO_KG + STD_CARGO_KG # average weight - ret.wheelbase = 2.82 + elif candidate == CAR.PILOT: + ret.mass = 4278. * CV.LB_TO_KG + STD_CARGO_KG # average weight + ret.wheelbase = 2.86 ret.centerToFront = ret.wheelbase * 0.428 - ret.steerRatio = 17.25 # as spec + ret.steerRatio = 16.0 # 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]] @@ -348,5 +348,5 @@ class CarInterface(CarInterfaceBase): # pass in a car.CarControl # to be called @ 100hz - def apply(self, c): - return self.CC.update(c, self.CS) + def apply(self, c, now_nanos): + return self.CC.update(c, self.CS, now_nanos) diff --git a/selfdrive/car/honda/radar_interface.py b/selfdrive/car/honda/radar_interface.py index 629ab01d4c..660be4c449 100755 --- a/selfdrive/car/honda/radar_interface.py +++ b/selfdrive/car/honda/radar_interface.py @@ -21,7 +21,7 @@ class RadarInterface(RadarInterfaceBase): self.track_id = 0 self.radar_fault = False self.radar_wrong_config = False - self.radar_off_can = CP.radarOffCan + self.radar_off_can = CP.radarUnavailable self.radar_ts = CP.radarTimeStep self.delay = int(round(0.1 / CP.radarTimeStep)) # 0.1s delay of radar diff --git a/selfdrive/car/honda/tests/test_honda.py b/selfdrive/car/honda/tests/test_honda.py new file mode 100755 index 0000000000..7a8c86fb0a --- /dev/null +++ b/selfdrive/car/honda/tests/test_honda.py @@ -0,0 +1,20 @@ +#!/usr/bin/env python3 +import re +import unittest + +from selfdrive.car.honda.values import FW_VERSIONS + +HONDA_FW_VERSION_RE = br"\d{5}-[A-Z0-9]{3}(-|,)[A-Z0-9]{4}(\x00){2}$" + + +class TestHondaFingerprint(unittest.TestCase): + def test_fw_version_format(self): + # Asserts all FW versions follow an expected format + for fw_by_ecu in FW_VERSIONS.values(): + for fws in fw_by_ecu.values(): + for fw in fws: + self.assertTrue(re.match(HONDA_FW_VERSION_RE, fw) is not None, fw) + + +if __name__ == "__main__": + unittest.main() diff --git a/selfdrive/car/honda/values.py b/selfdrive/car/honda/values.py index 2906b23bda..3d1c006aaa 100644 --- a/selfdrive/car/honda/values.py +++ b/selfdrive/car/honda/values.py @@ -4,9 +4,10 @@ from typing import Dict, List, Optional, Union from cereal import car from common.conversions import Conversions as CV +from panda.python import uds from selfdrive.car import dbc_dict from selfdrive.car.docs_definitions import CarFootnote, CarInfo, Column, Harness -from selfdrive.car.fw_query_definitions import FwQueryConfig, Request, StdQueries +from selfdrive.car.fw_query_definitions import FwQueryConfig, Request, StdQueries, p16 Ecu = car.CarParams.Ecu VisualAlert = car.CarControl.HUDControl.VisualAlert @@ -91,7 +92,6 @@ class CAR: ACURA_RDX = "ACURA RDX 2018" ACURA_RDX_3G = "ACURA RDX 2020" PILOT = "HONDA PILOT 2017" - PASSPORT = "HONDA PASSPORT 2021" RIDGELINE = "HONDA RIDGELINE 2017" INSIGHT = "HONDA INSIGHT 2019" HONDA_E = "HONDA E 2020" @@ -142,20 +142,47 @@ CAR_INFO: Dict[str, Optional[Union[HondaCarInfo, List[HondaCarInfo]]]] = { CAR.ODYSSEY_CHN: None, # Chinese version of Odyssey CAR.ACURA_RDX: HondaCarInfo("Acura RDX 2016-18", "AcuraWatch Plus", min_steer_speed=12. * CV.MPH_TO_MS), CAR.ACURA_RDX_3G: HondaCarInfo("Acura RDX 2019-22", "All", min_steer_speed=3. * CV.MPH_TO_MS), - CAR.PILOT: HondaCarInfo("Honda Pilot 2016-22", min_steer_speed=12. * CV.MPH_TO_MS), - CAR.PASSPORT: HondaCarInfo("Honda Passport 2019-21", "All", min_steer_speed=12. * CV.MPH_TO_MS), + CAR.PILOT: [ + HondaCarInfo("Honda Pilot 2016-22", min_steer_speed=12. * CV.MPH_TO_MS), + HondaCarInfo("Honda Passport 2019-21", "All", min_steer_speed=12. * CV.MPH_TO_MS), + ], CAR.RIDGELINE: HondaCarInfo("Honda Ridgeline 2017-22", min_steer_speed=12. * CV.MPH_TO_MS), CAR.INSIGHT: HondaCarInfo("Honda Insight 2019-22", "All", min_steer_speed=3. * CV.MPH_TO_MS), CAR.HONDA_E: HondaCarInfo("Honda e 2020", "All", min_steer_speed=3. * CV.MPH_TO_MS), } +HONDA_VERSION_REQUEST = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \ + p16(0xF112) +HONDA_VERSION_RESPONSE = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40]) + \ + p16(0xF112) + FW_QUERY_CONFIG = FwQueryConfig( requests=[ + # Currently used to fingerprint + Request( + [StdQueries.UDS_VERSION_REQUEST], + [StdQueries.UDS_VERSION_RESPONSE], + bus=1, + ), + + # Data collection requests: + # Log extra identifiers for current ECUs + Request( + [HONDA_VERSION_REQUEST], + [HONDA_VERSION_RESPONSE], + bus=1, + ), + # Query Nidec PT bus from camera for data collection Request( [StdQueries.UDS_VERSION_REQUEST], [StdQueries.UDS_VERSION_RESPONSE], + bus=0, ), ], + extra_ecus=[ + # The only other ECU on PT bus accessible by camera on radarless Civic + (Ecu.unknown, 0x18DAB3F1, None), + ], ) FW_VERSIONS = { @@ -1067,6 +1094,8 @@ FW_VERSIONS = { b'28101-5EZ-A060\x00\x00', b'28101-5EZ-A100\x00\x00', b'28101-5EZ-A210\x00\x00', + b'28101-5EZ-A600\x00\x00', + b'28101-5EZ-A430\x00\x00', ], (Ecu.programmedFuelInjection, 0x18da10f1, None): [ b'37805-RLV-4060\x00\x00', @@ -1078,6 +1107,9 @@ FW_VERSIONS = { b'37805-RLV-C520\x00\x00', b'37805-RLV-C530\x00\x00', b'37805-RLV-C910\x00\x00', + b'37805-RLV-B220\x00\x00', + b'37805-RLV-B210\x00\x00', + b'37805-RLV-L160\x00\x00', ], (Ecu.gateway, 0x18daeff1, None): [ b'38897-TG7-A030\x00\x00', @@ -1110,6 +1142,7 @@ FW_VERSIONS = { b'36161-TGS-A130\x00\x00', b'36161-TGT-A030\x00\x00', b'36161-TGT-A130\x00\x00', + b'36161-TGS-A030\x00\x00', ], (Ecu.srs, 0x18da53f1, None): [ b'77959-TG7-A020\x00\x00', @@ -1147,6 +1180,9 @@ FW_VERSIONS = { b'78109-TGS-AP20\x00\x00', b'78109-TGT-AJ20\x00\x00', b'78109-TGT-AK30\x00\x00', + b'78109-TGS-AT20\x00\x00', + b'78109-TGS-AX20\x00\x00', + b'78109-TGS-AJ20\x00\x00', ], (Ecu.vsa, 0x18da28f1, None): [ b'57114-TG7-A130\x00\x00', @@ -1163,42 +1199,6 @@ FW_VERSIONS = { b'57114-TGT-A530\x00\x00', ], }, - CAR.PASSPORT: { - (Ecu.programmedFuelInjection, 0x18da10f1, None): [ - b'37805-RLV-B220\x00\x00', - b'37805-RLV-B210\x00\x00', - b'37805-RLV-L160\x00\x00', - ], - (Ecu.eps, 0x18da30f1, None): [ - b'39990-TGS-A230\x00\x00', - ], - (Ecu.fwdRadar, 0x18dab0f1, None): [ - b'36161-TGS-A030\x00\x00', - b'36161-TGS-A130\x00\x00', - ], - (Ecu.gateway, 0x18daeff1, None): [ - b'38897-TG7-A040\x00\x00', - b'38897-TG7-A030\x00\x00', - ], - (Ecu.srs, 0x18da53f1, None): [ - b'77959-TGS-A010\x00\x00', - ], - (Ecu.shiftByWire, 0x18da0bf1, None): [ - b'54008-TG7-A530\x00\x00', - ], - (Ecu.transmission, 0x18da1ef1, None): [ - b'28101-5EZ-A600\x00\x00', - b'28101-5EZ-A430\x00\x00', - ], - (Ecu.combinationMeter, 0x18da60f1, None): [ - b'78109-TGS-AT20\x00\x00', - b'78109-TGS-AX20\x00\x00', - b'78109-TGS-AJ20\x00\x00', - ], - (Ecu.vsa, 0x18da28f1, None): [ - b'57114-TGS-A530\x00\x00', - ], - }, CAR.ACURA_RDX: { (Ecu.vsa, 0x18da28f1, None): [ b'57114-TX5-A220\x00\x00', @@ -1440,6 +1440,7 @@ FW_VERSIONS = { (Ecu.eps, 0x18DA30F1, None): [ b'39990-T39-A130\x00\x00', b'39990-T43-J020\x00\x00', + b'39990-T24-T120\x00\x00', ], (Ecu.gateway, 0x18DAEFF1, None): [ b'38897-T20-A020\x00\x00', @@ -1447,11 +1448,13 @@ FW_VERSIONS = { b'38897-T21-A010\x00\x00', b'38897-T20-A210\x00\x00', b'38897-T20-A310\x00\x00', + b'38897-T24-Z120\x00\x00', ], (Ecu.srs, 0x18DA53F1, None): [ b'77959-T20-A970\x00\x00', b'77959-T47-A940\x00\x00', b'77959-T47-A950\x00\x00', + b'77959-T20-M820\x00\x00', ], (Ecu.combinationMeter, 0x18DA60F1, None): [ b'78108-T21-A220\x00\x00', @@ -1459,16 +1462,26 @@ FW_VERSIONS = { b'78108-T23-A110\x00\x00', b'78108-T21-A230\x00\x00', b'78108-T22-A020\x00\x00', + b'78108-T21-MB10\x00\x00', + ], + (Ecu.fwdRadar, 0x18dab0f1, None): [ + b'36161-T20-A070\x00\x00', + b'36161-T20-A080\x00\x00', + b'36161-T20-A060\x00\x00', + b'36161-T47-A070\x00\x00', + b'36161-T24-T070\x00\x00', ], (Ecu.vsa, 0x18DA28F1, None): [ b'57114-T20-AB40\x00\x00', b'57114-T43-JB30\x00\x00', + b'57114-T24-TB30\x00\x00', ], (Ecu.transmission, 0x18da1ef1, None): [ b'28101-65D-A020\x00\x00', b'28101-65D-A120\x00\x00', b'28101-65H-A020\x00\x00', b'28101-65H-A120\x00\x00', + b'28101-65J-N010\x00\x00', ], (Ecu.programmedFuelInjection, 0x18da10f1, None): [ b'37805-64L-A540\x00\x00', @@ -1476,6 +1489,7 @@ FW_VERSIONS = { b'37805-64S-A720\x00\x00', b'37805-64A-A540\x00\x00', b'37805-64A-A620\x00\x00', + b'37805-64D-P510\x00\x00', ], }, } @@ -1499,7 +1513,6 @@ DBC = { 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('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), @@ -1514,7 +1527,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.PASSPORT, CAR.RIDGELINE} + CAR.PILOT, 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.CIVIC_2022} 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 b81c5e3f7d..7c31a48ba5 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -54,7 +54,7 @@ class CarController: self.car_fingerprint = CP.carFingerprint self.last_button_frame = 0 - def update(self, CC, CS): + def update(self, CC, CS, now_nanos): actuators = CC.actuators hud_control = CC.hudControl @@ -181,7 +181,8 @@ class CarController: if self.frame % 5 == 0 and self.car_fingerprint in (CAR.SONATA, CAR.PALISADE, CAR.IONIQ, CAR.KIA_NIRO_EV, CAR.KIA_NIRO_HEV_2021, CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV, CAR.KIA_CEED, CAR.KIA_SELTOS, CAR.KONA_EV, CAR.KONA_EV_2022, CAR.ELANTRA_2021, CAR.ELANTRA_HEV_2021, CAR.SONATA_HYBRID, CAR.KONA_HEV, CAR.SANTA_FE_2022, - CAR.KIA_K5_2021, CAR.IONIQ_HEV_2022, CAR.SANTA_FE_HEV_2022, CAR.GENESIS_G70_2020, CAR.SANTA_FE_PHEV_2022, CAR.KIA_STINGER_2022): + CAR.KIA_K5_2021, CAR.IONIQ_HEV_2022, CAR.SANTA_FE_HEV_2022, CAR.GENESIS_G70_2020, CAR.SANTA_FE_PHEV_2022, + CAR.KIA_STINGER_2022, CAR.KIA_K5_HEV_2020): can_sends.append(hyundaican.create_lfahda_mfc(self.packer, CC.enabled)) # 5 Hz ACC options diff --git a/selfdrive/car/hyundai/carstate.py b/selfdrive/car/hyundai/carstate.py index da1a7bfa78..0bab188790 100644 --- a/selfdrive/car/hyundai/carstate.py +++ b/selfdrive/car/hyundai/carstate.py @@ -6,6 +6,7 @@ from cereal import car from common.conversions import Conversions as CV from opendbc.can.parser import CANParser from opendbc.can.can_define import CANDefine +from selfdrive.car.hyundai.hyundaicanfd import get_e_can_bus from selfdrive.car.hyundai.values import HyundaiFlags, CAR, DBC, FEATURES, CAMERA_SCC_CAR, CANFD_CAR, EV_CAR, HYBRID_CAR, Buttons, CarControllerParams from selfdrive.car.interfaces import CarStateBase @@ -21,7 +22,9 @@ class CarState(CarStateBase): self.cruise_buttons = deque([Buttons.NONE] * PREV_BUTTON_SAMPLES, maxlen=PREV_BUTTON_SAMPLES) self.main_buttons = deque([Buttons.NONE] * PREV_BUTTON_SAMPLES, maxlen=PREV_BUTTON_SAMPLES) - self.gear_msg_canfd = "GEAR_ALT" if CP.flags & HyundaiFlags.CANFD_ALT_GEARS else "GEAR_SHIFTER" + self.gear_msg_canfd = "GEAR_ALT_2" if CP.flags & HyundaiFlags.CANFD_ALT_GEARS_2 else \ + "GEAR_ALT" if CP.flags & HyundaiFlags.CANFD_ALT_GEARS else \ + "GEAR_SHIFTER" if CP.carFingerprint in CANFD_CAR: self.shifter_values = can_define.dv[self.gear_msg_canfd]["GEAR"] elif self.CP.carFingerprint in FEATURES["use_cluster_gears"]: @@ -425,7 +428,9 @@ class CarState(CarStateBase): def get_can_parser_canfd(CP): cruise_btn_msg = "CRUISE_BUTTONS_ALT" if CP.flags & HyundaiFlags.CANFD_ALT_BUTTONS else "CRUISE_BUTTONS" - gear_msg = "GEAR_ALT" if CP.flags & HyundaiFlags.CANFD_ALT_GEARS else "GEAR_SHIFTER" + gear_msg = "GEAR_ALT_2" if CP.flags & HyundaiFlags.CANFD_ALT_GEARS_2 else \ + "GEAR_ALT" if CP.flags & HyundaiFlags.CANFD_ALT_GEARS else \ + "GEAR_SHIFTER" signals = [ ("WHEEL_SPEED_1", "WHEEL_SPEEDS"), ("WHEEL_SPEED_2", "WHEEL_SPEEDS"), @@ -511,8 +516,7 @@ class CarState(CarStateBase): ("ACCELERATOR_BRAKE_ALT", 100), ] - bus = 5 if CP.flags & HyundaiFlags.CANFD_HDA2 else 4 - return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, bus) + return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, get_e_can_bus(CP)) @staticmethod def get_cam_can_parser_canfd(CP): diff --git a/selfdrive/car/hyundai/hyundaican.py b/selfdrive/car/hyundai/hyundaican.py index c2ffffbf22..858f3d0876 100644 --- a/selfdrive/car/hyundai/hyundaican.py +++ b/selfdrive/car/hyundai/hyundaican.py @@ -21,7 +21,7 @@ def create_lkas11(packer, frame, car_fingerprint, apply_steer, steer_req, CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV, CAR.KIA_SELTOS, CAR.ELANTRA_2021, CAR.GENESIS_G70_2020, CAR.ELANTRA_HEV_2021, CAR.SONATA_HYBRID, CAR.KONA_EV, CAR.KONA_HEV, CAR.KONA_EV_2022, CAR.SANTA_FE_2022, CAR.KIA_K5_2021, CAR.IONIQ_HEV_2022, CAR.SANTA_FE_HEV_2022, - CAR.SANTA_FE_PHEV_2022, CAR.KIA_STINGER_2022): + CAR.SANTA_FE_PHEV_2022, CAR.KIA_STINGER_2022, CAR.KIA_K5_HEV_2020): values["CF_Lkas_LdwsActivemode"] = int(left_lane) + (int(right_lane) << 1) values["CF_Lkas_LdwsOpt_USM"] = 2 diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index 6d6d9833df..b746680930 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -20,7 +20,7 @@ class CarInterface(CarInterfaceBase): @staticmethod def _get_params(ret, candidate, fingerprint, car_fw, experimental_long): ret.carName = "hyundai" - ret.radarOffCan = RADAR_START_ADDR not in fingerprint[1] or DBC[ret.carFingerprint]["radar"] is None + ret.radarUnavailable = RADAR_START_ADDR not in fingerprint[1] or DBC[ret.carFingerprint]["radar"] is None # These cars have been put into dashcam only due to both a lack of users and test coverage. # These cars likely still work fine. Once a user confirms each car works and a test route is @@ -35,9 +35,12 @@ class CarInterface(CarInterfaceBase): # non-HDA2 if 0x1cf not in fingerprint[4]: ret.flags |= HyundaiFlags.CANFD_ALT_BUTTONS.value - # ICE cars do not have 0x130; GEARS message on 0x40 instead + # ICE cars do not have 0x130; GEARS message on 0x40 or 0x70 instead if 0x130 not in fingerprint[4]: - ret.flags |= HyundaiFlags.CANFD_ALT_GEARS.value + if 0x40 not in fingerprint[4]: + ret.flags |= HyundaiFlags.CANFD_ALT_GEARS_2.value + else: + ret.flags |= HyundaiFlags.CANFD_ALT_GEARS.value if candidate not in CANFD_RADAR_SCC_CAR: ret.flags |= HyundaiFlags.CANFD_CAMERA_SCC.value @@ -129,8 +132,8 @@ class CarInterface(CarInterfaceBase): ret.mass = 1985. + STD_CARGO_KG ret.wheelbase = 2.78 ret.steerRatio = 14.4 * 1.1 # 10% higher at the center seems reasonable - elif candidate in (CAR.KIA_NIRO_EV, CAR.KIA_NIRO_PHEV, CAR.KIA_NIRO_HEV_2021): - ret.mass = 1737. + STD_CARGO_KG + elif candidate in (CAR.KIA_NIRO_EV, CAR.KIA_NIRO_PHEV, CAR.KIA_NIRO_HEV_2021, CAR.KIA_NIRO_HEV_2ND_GEN): + ret.mass = 3452. * CV.LB_TO_KG + STD_CARGO_KG # average of all the cars ret.wheelbase = 2.7 ret.steerRatio = 13.9 if CAR.KIA_NIRO_HEV_2021 else 13.73 # Spec tire_stiffness_factor = 0.385 @@ -166,7 +169,7 @@ class CarInterface(CarInterfaceBase): ret.wheelbase = 2.65 ret.steerRatio = 13.75 tire_stiffness_factor = 0.5 - elif candidate == CAR.KIA_K5_2021: + elif candidate in (CAR.KIA_K5_2021, CAR.KIA_K5_HEV_2020): ret.mass = 3228. * CV.LB_TO_KG ret.wheelbase = 2.85 ret.steerRatio = 13.27 # 2021 Kia K5 Steering Ratio (all trims) @@ -327,5 +330,5 @@ class CarInterface(CarInterfaceBase): return ret - def apply(self, c): - return self.CC.update(c, self.CS) + def apply(self, c, now_nanos): + return self.CC.update(c, self.CS, now_nanos) diff --git a/selfdrive/car/hyundai/radar_interface.py b/selfdrive/car/hyundai/radar_interface.py index 0d22611fb5..4ecca542b5 100644 --- a/selfdrive/car/hyundai/radar_interface.py +++ b/selfdrive/car/hyundai/radar_interface.py @@ -37,7 +37,7 @@ class RadarInterface(RadarInterfaceBase): self.trigger_msg = RADAR_START_ADDR + RADAR_MSG_COUNT - 1 self.track_id = 0 - self.radar_off_can = CP.radarOffCan + self.radar_off_can = CP.radarUnavailable self.rcp = get_radar_can_parser(CP) def update(self, can_strings): diff --git a/selfdrive/car/hyundai/tests/test_hyundai.py b/selfdrive/car/hyundai/tests/test_hyundai.py index c20396ff76..0f419c1a2b 100755 --- a/selfdrive/car/hyundai/tests/test_hyundai.py +++ b/selfdrive/car/hyundai/tests/test_hyundai.py @@ -2,7 +2,7 @@ import unittest from cereal import car -from selfdrive.car.hyundai.values import CANFD_CAR, FW_VERSIONS, FW_QUERY_CONFIG +from selfdrive.car.hyundai.values import CANFD_CAR, FW_QUERY_CONFIG, FW_VERSIONS Ecu = car.CarParams.Ecu ECU_NAME = {v: k for k, v in Ecu.schema.enumerants.items()} diff --git a/selfdrive/car/hyundai/values.py b/selfdrive/car/hyundai/values.py index d363f7c901..e237f8d98e 100644 --- a/selfdrive/car/hyundai/values.py +++ b/selfdrive/car/hyundai/values.py @@ -35,7 +35,7 @@ class CarControllerParams: # To determine the limit for your car, find the maximum value that the stock LKAS will request. # If the max stock LKAS request is <384, add your car to this list. - elif CP.carFingerprint in (CAR.GENESIS_G80, CAR.GENESIS_G90, CAR.ELANTRA, CAR.HYUNDAI_GENESIS, CAR.IONIQ, + elif CP.carFingerprint in (CAR.GENESIS_G80, CAR.GENESIS_G90, CAR.ELANTRA, CAR.IONIQ, CAR.IONIQ_EV_LTD, CAR.SANTA_FE_PHEV_2022, CAR.SONATA_LF, CAR.KIA_FORTE, CAR.KIA_NIRO_PHEV, CAR.KIA_OPTIMA_H, CAR.KIA_SORENTO): self.STEER_MAX = 255 @@ -61,6 +61,8 @@ class HyundaiFlags(IntFlag): ENABLE_BLINKERS = 32 + CANFD_ALT_GEARS_2 = 64 + class CAR: # Hyundai @@ -96,9 +98,11 @@ class CAR: # Kia KIA_FORTE = "KIA FORTE E 2018 & GT 2021" KIA_K5_2021 = "KIA K5 2021" + KIA_K5_HEV_2020 = "KIA K5 HYBRID 2020" KIA_NIRO_EV = "KIA NIRO EV 2020" KIA_NIRO_PHEV = "KIA NIRO HYBRID 2019" KIA_NIRO_HEV_2021 = "KIA NIRO HYBRID 2021" + KIA_NIRO_HEV_2ND_GEN = "KIA NIRO HYBRID 2ND GEN" KIA_OPTIMA_G4 = "KIA OPTIMA 4TH GEN" KIA_OPTIMA_G4_FL = "KIA OPTIMA 4TH GEN FACELIFT" KIA_OPTIMA_H = "KIA OPTIMA HYBRID 2017 & SPORTS 2019" @@ -145,9 +149,12 @@ CAR_INFO: Dict[str, Optional[Union[HyundaiCarInfo, List[HyundaiCarInfo]]]] = { HyundaiCarInfo("Hyundai Elantra GT 2017-19", harness=Harness.hyundai_e), HyundaiCarInfo("Hyundai i30 2017-19", harness=Harness.hyundai_e), ], - CAR.ELANTRA_2021: HyundaiCarInfo("Hyundai Elantra 2021-22", video_link="https://youtu.be/_EdYQtV52-c", harness=Harness.hyundai_k), + CAR.ELANTRA_2021: HyundaiCarInfo("Hyundai Elantra 2021-23", video_link="https://youtu.be/_EdYQtV52-c", harness=Harness.hyundai_k), CAR.ELANTRA_HEV_2021: HyundaiCarInfo("Hyundai Elantra Hybrid 2021-23", video_link="https://youtu.be/_EdYQtV52-c", harness=Harness.hyundai_k), - CAR.HYUNDAI_GENESIS: HyundaiCarInfo("Hyundai Genesis 2015-16", min_enable_speed=19 * CV.MPH_TO_MS, harness=Harness.hyundai_j), # TODO: check 2015 packages + CAR.HYUNDAI_GENESIS: [ + HyundaiCarInfo("Hyundai Genesis 2015-16", min_enable_speed=19 * CV.MPH_TO_MS, harness=Harness.hyundai_j), # TODO: check 2015 packages + HyundaiCarInfo("Genesis G80 2017", "All", min_enable_speed=19 * CV.MPH_TO_MS, harness=Harness.hyundai_j), + ], CAR.IONIQ: HyundaiCarInfo("Hyundai Ioniq Hybrid 2017-19", harness=Harness.hyundai_c), CAR.IONIQ_HEV_2022: HyundaiCarInfo("Hyundai Ioniq Hybrid 2020-22", harness=Harness.hyundai_h), # TODO: confirm 2020-21 harness CAR.IONIQ_EV_LTD: HyundaiCarInfo("Hyundai Ioniq Electric 2019", harness=Harness.hyundai_c), @@ -189,17 +196,22 @@ CAR_INFO: Dict[str, Optional[Union[HyundaiCarInfo, List[HyundaiCarInfo]]]] = { # Kia CAR.KIA_FORTE: HyundaiCarInfo("Kia Forte 2019-21", harness=Harness.hyundai_g), CAR.KIA_K5_2021: HyundaiCarInfo("Kia K5 2021-22", harness=Harness.hyundai_a), + CAR.KIA_K5_HEV_2020: HyundaiCarInfo("Kia K5 Hybrid 2020", harness=Harness.hyundai_a), CAR.KIA_NIRO_EV: [ HyundaiCarInfo("Kia Niro EV 2019", "All", "https://www.youtube.com/watch?v=lT7zcG6ZpGo", harness=Harness.hyundai_h), HyundaiCarInfo("Kia Niro EV 2020", "All", "https://www.youtube.com/watch?v=lT7zcG6ZpGo", harness=Harness.hyundai_f), HyundaiCarInfo("Kia Niro EV 2021", "All", "https://www.youtube.com/watch?v=lT7zcG6ZpGo", harness=Harness.hyundai_c), HyundaiCarInfo("Kia Niro EV 2022", "All", "https://www.youtube.com/watch?v=lT7zcG6ZpGo", harness=Harness.hyundai_h), ], - CAR.KIA_NIRO_PHEV: HyundaiCarInfo("Kia Niro Plug-in Hybrid 2018-19", "All", min_enable_speed=10. * CV.MPH_TO_MS, harness=Harness.hyundai_c), + CAR.KIA_NIRO_PHEV: [ + HyundaiCarInfo("Kia Niro Plug-in Hybrid 2018-19", "All", min_enable_speed=10. * CV.MPH_TO_MS, harness=Harness.hyundai_c), + HyundaiCarInfo("Kia Niro Plug-in Hybrid 2020", "All", harness=Harness.hyundai_d), + ], CAR.KIA_NIRO_HEV_2021: [ HyundaiCarInfo("Kia Niro Hybrid 2021", harness=Harness.hyundai_f), # TODO: could be hyundai_d, verify HyundaiCarInfo("Kia Niro Hybrid 2022", harness=Harness.hyundai_h), ], + CAR.KIA_NIRO_HEV_2ND_GEN: HyundaiCarInfo("Kia Niro Hybrid 2023", harness=Harness.hyundai_a), CAR.KIA_OPTIMA_G4: HyundaiCarInfo("Kia Optima 2017", "Advanced Smart Cruise Control", harness=Harness.hyundai_b), # TODO: may support 2016, 2018 CAR.KIA_OPTIMA_G4_FL: HyundaiCarInfo("Kia Optima 2019-20", harness=Harness.hyundai_g), CAR.KIA_OPTIMA_H: [ @@ -225,11 +237,14 @@ CAR_INFO: Dict[str, Optional[Union[HyundaiCarInfo, List[HyundaiCarInfo]]]] = { ], # Genesis - CAR.GENESIS_GV60_EV_1ST_GEN: HyundaiCarInfo("Genesis GV60 2023", "All", harness=Harness.hyundai_k), + CAR.GENESIS_GV60_EV_1ST_GEN: [ + HyundaiCarInfo("Genesis GV60 (Advanced Trim) 2023", "All", harness=Harness.hyundai_a), + HyundaiCarInfo("Genesis GV60 (Performance Trim) 2023", "All", harness=Harness.hyundai_k), + ], CAR.GENESIS_G70: HyundaiCarInfo("Genesis G70 2018-19", "All", harness=Harness.hyundai_f), CAR.GENESIS_G70_2020: HyundaiCarInfo("Genesis G70 2020", "All", harness=Harness.hyundai_f), CAR.GENESIS_GV70_1ST_GEN: HyundaiCarInfo("Genesis GV70 2022-23", "All", harness=Harness.hyundai_l), - CAR.GENESIS_G80: HyundaiCarInfo("Genesis G80 2017-19", "All", harness=Harness.hyundai_h), + CAR.GENESIS_G80: HyundaiCarInfo("Genesis G80 2018-19", "All", harness=Harness.hyundai_h), CAR.GENESIS_G90: HyundaiCarInfo("Genesis G90 2017-18", "All", harness=Harness.hyundai_c), } @@ -394,6 +409,13 @@ FW_QUERY_CONFIG = FwQueryConfig( ) FW_VERSIONS = { + CAR.HYUNDAI_GENESIS: { + (Ecu.fwdCamera, 0x7c4, None): [ + b'\xf1\x00DH LKAS 1.1 -150210', + b'\xf1\x00DH LKAS 1.4 -140110', + b'\xf1\x00DH LKAS 1.5 -140425', + ], + }, CAR.IONIQ: { (Ecu.fwdRadar, 0x7d0, None): [ b'\xf1\x00AEhe SCC H-CUP 1.01 1.01 96400-G2000 ', @@ -431,17 +453,18 @@ FW_VERSIONS = { }, CAR.IONIQ_PHEV: { (Ecu.fwdRadar, 0x7d0, None): [ - b'\xf1\000AEhe SCC FHCUP 1.00 1.02 99110-G2100 ', + b'\xf1\x00AEhe SCC FHCUP 1.00 1.02 99110-G2100 ', b'\xf1\x00AEhe SCC F-CUP 1.00 1.00 99110-G2200 ', b'\xf1\x00AEhe SCC F-CUP 1.00 1.00 99110-G2600 ', + b'\xf1\x00AEhe SCC F-CUP 1.00 1.02 99110-G2100 ', ], (Ecu.eps, 0x7d4, None): [ - b'\xf1\000AE MDPS C 1.00 1.01 56310/G2510 4APHC101', + b'\xf1\x00AE MDPS C 1.00 1.01 56310/G2510 4APHC101', b'\xf1\x00AE MDPS C 1.00 1.01 56310/G2560 4APHC101', b'\xf1\x00AE MDPS C 1.00 1.01 56310G2510\x00 4APHC101', ], (Ecu.fwdCamera, 0x7c4, None): [ - b'\xf1\000AEP MFC AT USA LHD 1.00 1.01 95740-G2600 190819', + b'\xf1\x00AEP MFC AT USA LHD 1.00 1.01 95740-G2600 190819', b'\xf1\x00AEP MFC AT EUR RHD 1.00 1.01 95740-G2600 190819', b'\xf1\x00AEP MFC AT USA LHD 1.00 1.00 95740-G2700 201027', ], @@ -454,6 +477,7 @@ FW_VERSIONS = { b'\xf1\x816U3J8051\x00\x00\xf1\x006U3H1_C2\x00\x006U3J8051\x00\x00PAETG16UL0\x00\x00\x00\x00', b'\xf1\x816U3J9051\x00\x00\xf1\x006U3H1_C2\x00\x006U3J9051\x00\x00PAE0G16NL2\xad\xeb\xabt', b'\xf1\x816U3J9051\x00\x00\xf1\x006U3H1_C2\x00\x006U3J9051\x00\x00PAE0G16NL2\x00\x00\x00\x00', + b'\xf1\x006U3H1_C2\x00\x006U3J9051\x00\x00PAE0G16NL0\x00\x00\x00\x00', ], }, CAR.IONIQ_EV_2020: { @@ -481,11 +505,13 @@ FW_VERSIONS = { b'\xf1\x00AE MDPS C 1.00 1.04 56310/G7501 4AEEC104', b'\xf1\x00AE MDPS C 1.00 1.03 56310/G7300 4AEEC103', b'\xf1\x00AE MDPS C 1.00 1.03 56310G7300\x00 4AEEC103', + b'\xf1\x00AE MDPS C 1.00 1.04 56310/G7301 4AEEC104', ], (Ecu.fwdCamera, 0x7c4, None): [ b'\xf1\x00AEE MFC AT EUR LHD 1.00 1.00 95740-G7200 160418', b'\xf1\x00AEE MFC AT USA LHD 1.00 1.00 95740-G2400 180222', b'\xf1\x00AEE MFC AT EUR LHD 1.00 1.00 95740-G2300 170703', + b'\xf1\x00AEE MFC AT EUR LHD 1.00 1.00 95740-G2400 180222', ], }, CAR.IONIQ_HEV_2022: { @@ -695,7 +721,7 @@ FW_VERSIONS = { (Ecu.abs, 0x7d1, None): [ b'\xf1\x00TM ESC \r 100\x18\x031 58910-S2650', b'\xf1\x00TM ESC \r 103\x18\x11\x08 58910-S2650', - b'\xf1\x00TM ESC \r 104\x19\a\b 58910-S2650', + b'\xf1\x00TM ESC \r 104\x19\x07\x08 58910-S2650', b'\xf1\x00TM ESC \x02 100\x18\x030 58910-S2600', b'\xf1\x00TM ESC \x02 102\x18\x07\x01 58910-S2600', b'\xf1\x00TM ESC \x02 103\x18\x11\x07 58910-S2600', @@ -717,6 +743,7 @@ FW_VERSIONS = { b'\xf1\x00TM MFC AT USA LHD 1.00 1.00 99211-S2000 180409', ], (Ecu.transmission, 0x7e1, None): [ + b'\xf1\x006W351_C2\x00\x006W3E1051\x00\x00TTM4T20NS5\x00\x00\x00\x00', b'\xf1\x87LBJSGA7082574HG0\x87www\x98\x88\x88\x88\x99\xaa\xb9\x9afw\x86gx\x99\xa7\x89co\xf8\xffvU_\xffR\xaf\xf1\x816W3C2051\x00\x00\xf1\x006W351_C2\x00\x006W3C2051\x00\x00TTM2T20NS1\x00\xa6\xe0\x91', b'\xf1\x87LBKSGA0458404HG0vfvg\x87www\x89\x99\xa8\x99y\xaa\xa7\x9ax\x88\xa7\x88t_\xf9\xff\x86w\x8f\xff\x15x\xf1\x816W3C2051\x00\x00\xf1\x006W351_C2\x00\x006W3C2051\x00\x00TTM2T20NS1\x00\x00\x00\x00', b'\xf1\x87LDJUEA6010814HG1\x87w\x87x\x86gvw\x88\x88\x98\x88gw\x86wx\x88\x97\x88\x85o\xf8\xff\x86f_\xff\xd37\xf1\x816W3C2051\x00\x00\xf1\x006W351_C2\x00\x006W3C2051\x00\x00TTM4T20NS0\xf8\x19\x92g', @@ -795,19 +822,23 @@ FW_VERSIONS = { }, CAR.SANTA_FE_HEV_2022: { (Ecu.fwdRadar, 0x7d0, None): [ - b'\xf1\x8799110CL500\xf1\x00TMhe SCC FHCUP 1.00 1.00 99110-CL500 ', + b'\xf1\x00TMhe SCC FHCUP 1.00 1.00 99110-CL500 ', ], (Ecu.eps, 0x7d4, None): [ b'\xf1\x00TM MDPS C 1.00 1.02 56310-CLAC0 4TSHC102', + b'\xf1\x00TM MDPS R 1.00 1.05 57700-CL000 4TSHP105', ], (Ecu.fwdCamera, 0x7c4, None): [ + b'\xf1\x00TMH MFC AT EUR LHD 1.00 1.06 99211-S1500 220727', b'\xf1\x00TMH MFC AT USA LHD 1.00 1.03 99211-S1500 210224', ], (Ecu.transmission, 0x7e1, None): [ + b'\xf1\x00PSBG2333 E16\x00\x00\x00\x00\x00\x00\x00TTM2H16UA3I\x94\xac\x8f', b'\xf1\x87959102T250\x00\x00\x00\x00\x00\xf1\x81E14\x00\x00\x00\x00\x00\x00\x00\xf1\x00PSBG2333 E14\x00\x00\x00\x00\x00\x00\x00TTM2H16SA2\x80\xd7l\xb2', ], (Ecu.engine, 0x7e0, None): [ b'\xf1\x87391312MTC1', + b'\xf1\x87391312MTE0', ], }, CAR.SANTA_FE_PHEV_2022: { @@ -1050,6 +1081,25 @@ FW_VERSIONS = { b'\xf1\x81640H0051\x00\x00\x00\x00\x00\x00\x00\x00', ], }, + CAR.GENESIS_G80: { + (Ecu.fwdRadar, 0x7d0, None): [ + b'\xf1\x00DH__ SCC F-CUP 1.00 1.01 96400-B1120 ', + ], + (Ecu.fwdCamera, 0x7c4, None): [ + b'\xf1\x00DH LKAS AT USA LHD 1.01 1.03 95895-B1500 180713', + b'\xf1\x00DH LKAS AT USA LHD 1.01 1.02 95895-B1500 170810', + b'\xf1\x00DH LKAS AT USA LHD 1.01 1.01 95895-B1500 161014', + ], + (Ecu.transmission, 0x7e1, None): [ + b'\xf1\x00bcsh8p54 E21\x00\x00\x00\x00\x00\x00\x00SDH0T33NH4\xd7O\x9e\xc9', + b'\xf1\x00bcsh8p54 E18\x00\x00\x00\x00\x00\x00\x00TDH0G38NH3:-\xa9n', + b'\xf1\x00bcsh8p54 E18\x00\x00\x00\x00\x00\x00\x00SDH0G38NH2j\x9dA\x1c', + b'\xf1\x00bcsh8p54 E18\x00\x00\x00\x00\x00\x00\x00SDH0T33NH3\x97\xe6\xbc\xb8', + ], + (Ecu.engine, 0x7e0, None): [ + b'\xf1\x81640F0051\x00\x00\x00\x00\x00\x00\x00\x00', + ], + }, CAR.GENESIS_G90: { (Ecu.transmission, 0x7e1, None): [b'\xf1\x87VDGMD15866192DD3x\x88x\x89wuFvvfUf\x88vWwgwwwvfVgx\x87o\xff\xbc^\xf1\x81E14\x00\x00\x00\x00\x00\x00\x00\xf1\x00bcshcm49 E14\x00\x00\x00\x00\x00\x00\x00SHI0G50NB1tc5\xb7'], (Ecu.fwdRadar, 0x7d0, None): [b'\xf1\x00HI__ SCC F-CUP 1.00 1.01 96400-D2100 '], @@ -1138,6 +1188,23 @@ FW_VERSIONS = { b'\xf1\x00bcsh8p54 U913\x00\x00\x00\x00\x00\x00TDL4T16NB05\x94t\x18', ], }, + CAR.KIA_K5_HEV_2020: { + (Ecu.fwdRadar, 0x7D0, None): [ + b'\xf1\x00DLhe SCC FHCUP 1.00 1.02 99110-L7000 ', + ], + (Ecu.eps, 0x7D4, None): [ + b'\xf1\x00DL3 MDPS C 1.00 1.02 56310-L7000 4DLHC102', + ], + (Ecu.fwdCamera, 0x7C4, None): [ + b'\xf1\x00DL3HMFC AT KOR LHD 1.00 1.02 99210-L2000 200309', + ], + (Ecu.engine, 0x7E0, None): [ + b'\xf1\x87391162JLA0', + ], + (Ecu.transmission, 0x7E1, None): [ + b'\xf1\x00PSBG2323 E08\x00\x00\x00\x00\x00\x00\x00TDL2H20KA2\xe3\xc6cz', + ], + }, CAR.KONA_EV: { (Ecu.abs, 0x7D1, None): [ b'\xf1\x00OS IEB \r 105\x18\t\x18 58520-K4000', @@ -1224,22 +1291,27 @@ FW_VERSIONS = { (Ecu.engine, 0x7e0, None): [ b'\xf1\x816H6F4051\x00\x00\x00\x00\x00\x00\x00\x00', b'\xf1\x816H6D1051\x00\x00\x00\x00\x00\x00\x00\x00', + b'\xf1\x816H6F6051\x00\x00\x00\x00\x00\x00\x00\x00', ], (Ecu.transmission, 0x7e1, None): [ b"\xf1\x816U3J2051\x00\x00\xf1\x006U3H0_C2\x00\x006U3J2051\x00\x00PDE0G16NS2\xf4'\\\x91", b'\xf1\x816U3J2051\x00\x00\xf1\x006U3H0_C2\x00\x006U3J2051\x00\x00PDE0G16NS2\x00\x00\x00\x00', b'\xf1\x816U3H3051\x00\x00\xf1\x006U3H0_C2\x00\x006U3H3051\x00\x00PDE0G16NS1\x00\x00\x00\x00', b'\xf1\x816U3H3051\x00\x00\xf1\x006U3H0_C2\x00\x006U3H3051\x00\x00PDE0G16NS1\x13\xcd\x88\x92', + b'\xf1\x006U3H1_C2\x00\x006U3J9051\x00\x00PDE0G16NL2&[\xc3\x01', ], (Ecu.eps, 0x7D4, None): [ b'\xf1\x00DE MDPS C 1.00 1.09 56310G5301\x00 4DEHC109', + b'\xf1\x00DE MDPS C 1.00 1.01 56310G5520\x00 4DEPC101', ], (Ecu.fwdCamera, 0x7C4, None): [ b'\xf1\x00DEP MFC AT USA LHD 1.00 1.01 95740-G5010 170424', b'\xf1\x00DEP MFC AT USA LHD 1.00 1.00 95740-G5010 170117', + b'\xf1\x00DEP MFC AT USA LHD 1.00 1.05 99211-G5000 190826', ], (Ecu.fwdRadar, 0x7D0, None): [ b'\xf1\x00DEhe SCC H-CUP 1.01 1.02 96400-G5100 ', + b'\xf1\x00DEhe SCC F-CUP 1.00 1.02 99110-G5100 ', ], }, CAR.KIA_NIRO_HEV_2021: { @@ -1350,25 +1422,26 @@ FW_VERSIONS = { (Ecu.fwdRadar, 0x7d0, None): [ b'\xf1\x00CN7_ SCC F-CUP 1.00 1.01 99110-AA000 ', b'\xf1\x00CN7_ SCC FHCUP 1.00 1.01 99110-AA000 ', + b'\xf1\x00CN7_ SCC FNCUP 1.00 1.01 99110-AA000 ', b'\xf1\x8799110AA000\xf1\x00CN7_ SCC FHCUP 1.00 1.01 99110-AA000 ', b'\xf1\x8799110AA000\xf1\x00CN7_ SCC F-CUP 1.00 1.01 99110-AA000 ', ], (Ecu.eps, 0x7d4, None): [ b'\xf1\x87\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\xf1\x00CN7 MDPS C 1.00 1.06 \x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00 4CNDC106', b'\xf1\x8756310/AA070\xf1\x00CN7 MDPS C 1.00 1.06 56310/AA070 4CNDC106', - b'\xf1\x8756310AA050\x00\xf1\x00CN7 MDPS C 1.00 1.06 56310AA050\x00 4CNDC106', b'\xf1\x8756310AA050\x00\xf1\x00CN7 MDPS C 1.00 1.06 56310AA050\x00 4CNDC106\xf1\xa01.06', + b'\xf1\x00CN7 MDPS C 1.00 1.06 56310AA050\x00 4CNDC106', ], (Ecu.fwdCamera, 0x7c4, None): [ b'\xf1\x00CN7 MFC AT USA LHD 1.00 1.00 99210-AB000 200819', b'\xf1\x00CN7 MFC AT USA LHD 1.00 1.03 99210-AA000 200819', b'\xf1\x00CN7 MFC AT USA LHD 1.00 1.01 99210-AB000 210205', b'\xf1\x00CN7 MFC AT USA LHD 1.00 1.06 99210-AA000 220111', + b'\xf1\x00CN7 MFC AT USA LHD 1.00 1.03 99210-AB000 220426', ], (Ecu.abs, 0x7d1, None): [ b'\xf1\x00CN ESC \t 101 \x10\x03 58910-AB800', b'\xf1\x8758910-AA800\xf1\x00CN ESC \t 104 \x08\x03 58910-AA800', - b'\xf1\x8758910-AB800\xf1\x00CN ESC \t 101 \x10\x03 58910-AB800', b'\xf1\x8758910-AA800\xf1\x00CN ESC \t 105 \x10\x03 58910-AA800', b'\xf1\x8758910-AB800\xf1\x00CN ESC \t 101 \x10\x03 58910-AB800\xf1\xa01.01', ], @@ -1386,6 +1459,7 @@ FW_VERSIONS = { b'\xf1\x81HM6M2_0a0_FF0', b'\xf1\x82CNCVD0AMFCXCSFFB', b'\xf1\x870\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\xf1\x81HM6M2_0a0_G80', + b'\xf1\x870\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\xf1\x81HM6M2_0a0_HC0', ], }, CAR.ELANTRA_HEV_2021: { @@ -1511,6 +1585,8 @@ FW_VERSIONS = { b'\xf1\x00NE1 MFC AT EUR LHD 1.00 1.06 99211-GI000 210813', b'\xf1\x00NE1 MFC AT USA LHD 1.00 1.05 99211-GI010 220614', b'\xf1\x00NE1 MFC AT EUR RHD 1.00 1.01 99211-GI010 211007', + b'\xf1\x00NE1 MFC AT USA LHD 1.00 1.01 99211-GI010 211007', + b'\xf1\x00NE1 MFC AT EUR RHD 1.00 1.02 99211-GI010 211206', ], }, CAR.TUCSON_4TH_GEN: { @@ -1571,6 +1647,7 @@ FW_VERSIONS = { CAR.GENESIS_GV60_EV_1ST_GEN: { (Ecu.fwdCamera, 0x7c4, None): [ b'\xf1\x00JW1 MFC AT USA LHD 1.00 1.02 99211-CU100 211215', + b'\xf1\x00JW1 MFC AT USA LHD 1.00 1.02 99211-CU000 211215', ], (Ecu.fwdRadar, 0x7d0, None): [ b'\xf1\x00JW1_ RDR ----- 1.00 1.00 99110-CU000 ', @@ -1584,10 +1661,18 @@ FW_VERSIONS = { b'\xf1\x00MQ4_ SCC FHCUP 1.00 1.06 99110-P2000 ', ], }, + CAR.KIA_NIRO_HEV_2ND_GEN: { + (Ecu.fwdCamera, 0x7c4, None): [ + b'\xf1\x00SG2HMFC AT USA LHD 1.01 1.08 99211-AT000 220531', + ], + (Ecu.fwdRadar, 0x7d0, None): [ + b'\xf1\x00SG2_ RDR ----- 1.00 1.01 99110-AT000 ', + ], + }, } CHECKSUM = { - "crc8": [CAR.SANTA_FE, CAR.SONATA, CAR.PALISADE, CAR.KIA_SELTOS, CAR.ELANTRA_2021, CAR.ELANTRA_HEV_2021, CAR.SONATA_HYBRID, CAR.SANTA_FE_2022, CAR.KIA_K5_2021, CAR.SANTA_FE_HEV_2022, CAR.SANTA_FE_PHEV_2022], + "crc8": [CAR.SANTA_FE, CAR.SONATA, CAR.PALISADE, CAR.KIA_SELTOS, CAR.ELANTRA_2021, CAR.ELANTRA_HEV_2021, CAR.SONATA_HYBRID, CAR.SANTA_FE_2022, CAR.KIA_K5_2021, CAR.SANTA_FE_HEV_2022, CAR.SANTA_FE_PHEV_2022, CAR.KIA_K5_HEV_2020], "6B": [CAR.KIA_SORENTO, CAR.HYUNDAI_GENESIS], } @@ -1595,13 +1680,13 @@ FEATURES = { # which message has the gear "use_cluster_gears": {CAR.ELANTRA, CAR.KONA}, "use_tcu_gears": {CAR.KIA_OPTIMA_G4, CAR.KIA_OPTIMA_G4_FL, CAR.SONATA_LF, CAR.VELOSTER, CAR.TUCSON}, - "use_elect_gears": {CAR.KIA_NIRO_EV, CAR.KIA_NIRO_PHEV, CAR.KIA_NIRO_HEV_2021, CAR.KIA_OPTIMA_H, CAR.IONIQ_EV_LTD, CAR.KONA_EV, CAR.IONIQ, CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV, CAR.ELANTRA_HEV_2021, CAR.SONATA_HYBRID, CAR.KONA_HEV, CAR.IONIQ_HEV_2022, CAR.SANTA_FE_HEV_2022, CAR.SANTA_FE_PHEV_2022, CAR.IONIQ_PHEV_2019, CAR.KONA_EV_2022}, + "use_elect_gears": {CAR.KIA_NIRO_EV, CAR.KIA_NIRO_PHEV, CAR.KIA_NIRO_HEV_2021, CAR.KIA_OPTIMA_H, CAR.IONIQ_EV_LTD, CAR.KONA_EV, CAR.IONIQ, CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV, CAR.ELANTRA_HEV_2021, CAR.SONATA_HYBRID, CAR.KONA_HEV, CAR.IONIQ_HEV_2022, CAR.SANTA_FE_HEV_2022, CAR.SANTA_FE_PHEV_2022, CAR.IONIQ_PHEV_2019, CAR.KONA_EV_2022, CAR.KIA_K5_HEV_2020}, # these cars use the FCA11 message for the AEB and FCW signals, all others use SCC12 - "use_fca": {CAR.SONATA, CAR.SONATA_HYBRID, CAR.ELANTRA, CAR.ELANTRA_2021, CAR.ELANTRA_HEV_2021, CAR.KIA_STINGER, CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV, CAR.KONA_EV, CAR.KIA_FORTE, CAR.KIA_NIRO_EV, CAR.PALISADE, CAR.GENESIS_G70, CAR.GENESIS_G70_2020, CAR.KONA, CAR.SANTA_FE, CAR.KIA_SELTOS, CAR.KONA_HEV, CAR.SANTA_FE_2022, CAR.KIA_K5_2021, CAR.IONIQ_HEV_2022, CAR.SANTA_FE_HEV_2022, CAR.SANTA_FE_PHEV_2022, CAR.TUCSON, CAR.KONA_EV_2022, CAR.KIA_STINGER_2022}, + "use_fca": {CAR.SONATA, CAR.SONATA_HYBRID, CAR.ELANTRA, CAR.ELANTRA_2021, CAR.ELANTRA_HEV_2021, CAR.KIA_STINGER, CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV, CAR.KONA_EV, CAR.KIA_FORTE, CAR.KIA_NIRO_EV, CAR.PALISADE, CAR.GENESIS_G70, CAR.GENESIS_G70_2020, CAR.KONA, CAR.SANTA_FE, CAR.KIA_SELTOS, CAR.KONA_HEV, CAR.SANTA_FE_2022, CAR.KIA_K5_2021, CAR.IONIQ_HEV_2022, CAR.SANTA_FE_HEV_2022, CAR.SANTA_FE_PHEV_2022, CAR.TUCSON, CAR.KONA_EV_2022, CAR.KIA_STINGER_2022, CAR.KIA_K5_HEV_2020}, } -CANFD_CAR = {CAR.KIA_EV6, CAR.IONIQ_5, CAR.TUCSON_4TH_GEN, CAR.TUCSON_HYBRID_4TH_GEN, CAR.KIA_SPORTAGE_HYBRID_5TH_GEN, CAR.SANTA_CRUZ_1ST_GEN, CAR.KIA_SPORTAGE_5TH_GEN, CAR.GENESIS_GV70_1ST_GEN, CAR.KIA_SORENTO_PHEV_4TH_GEN, CAR.GENESIS_GV60_EV_1ST_GEN, CAR.KIA_SORENTO_4TH_GEN} +CANFD_CAR = {CAR.KIA_EV6, CAR.IONIQ_5, CAR.TUCSON_4TH_GEN, CAR.TUCSON_HYBRID_4TH_GEN, CAR.KIA_SPORTAGE_HYBRID_5TH_GEN, CAR.SANTA_CRUZ_1ST_GEN, CAR.KIA_SPORTAGE_5TH_GEN, CAR.GENESIS_GV70_1ST_GEN, CAR.KIA_SORENTO_PHEV_4TH_GEN, CAR.GENESIS_GV60_EV_1ST_GEN, CAR.KIA_SORENTO_4TH_GEN, CAR.KIA_NIRO_HEV_2ND_GEN} # The radar does SCC on these cars when HDA I, rather than the camera CANFD_RADAR_SCC_CAR = {CAR.GENESIS_GV70_1ST_GEN, CAR.KIA_SORENTO_PHEV_4TH_GEN, CAR.KIA_SORENTO_4TH_GEN} @@ -1609,11 +1694,12 @@ CANFD_RADAR_SCC_CAR = {CAR.GENESIS_GV70_1ST_GEN, CAR.KIA_SORENTO_PHEV_4TH_GEN, C # The camera does SCC on these cars, rather than the radar CAMERA_SCC_CAR = {CAR.KONA_EV_2022, } -HYBRID_CAR = {CAR.IONIQ_PHEV, CAR.ELANTRA_HEV_2021, CAR.KIA_NIRO_PHEV, CAR.KIA_NIRO_HEV_2021, CAR.SONATA_HYBRID, CAR.KONA_HEV, CAR.IONIQ, CAR.IONIQ_HEV_2022, CAR.SANTA_FE_HEV_2022, CAR.SANTA_FE_PHEV_2022, CAR.IONIQ_PHEV_2019, CAR.TUCSON_HYBRID_4TH_GEN, CAR.KIA_SPORTAGE_HYBRID_5TH_GEN, CAR.KIA_SORENTO_PHEV_4TH_GEN} # these cars use a different gas signal +HYBRID_CAR = {CAR.IONIQ_PHEV, CAR.ELANTRA_HEV_2021, CAR.KIA_NIRO_PHEV, CAR.KIA_NIRO_HEV_2021, CAR.SONATA_HYBRID, CAR.KONA_HEV, CAR.IONIQ, CAR.IONIQ_HEV_2022, CAR.SANTA_FE_HEV_2022, CAR.SANTA_FE_PHEV_2022, CAR.IONIQ_PHEV_2019, CAR.TUCSON_HYBRID_4TH_GEN, CAR.KIA_SPORTAGE_HYBRID_5TH_GEN, CAR.KIA_SORENTO_PHEV_4TH_GEN, CAR.KIA_K5_HEV_2020, CAR.KIA_NIRO_HEV_2ND_GEN} # these cars use a different gas signal EV_CAR = {CAR.IONIQ_EV_2020, CAR.IONIQ_EV_LTD, CAR.KONA_EV, CAR.KIA_NIRO_EV, CAR.KONA_EV_2022, CAR.KIA_EV6, CAR.IONIQ_5, CAR.GENESIS_GV60_EV_1ST_GEN} # these cars require a special panda safety mode due to missing counters and checksums in the messages -LEGACY_SAFETY_MODE_CAR = {CAR.HYUNDAI_GENESIS, CAR.IONIQ_EV_2020, CAR.IONIQ_EV_LTD, CAR.IONIQ_PHEV, CAR.IONIQ, CAR.KONA_EV, CAR.KIA_SORENTO, CAR.SONATA_LF, CAR.KIA_OPTIMA_G4, CAR.KIA_OPTIMA_G4_FL, CAR.VELOSTER, CAR.KIA_STINGER, CAR.GENESIS_G70, CAR.GENESIS_G80, CAR.KIA_CEED, CAR.ELANTRA, CAR.IONIQ_HEV_2022} +LEGACY_SAFETY_MODE_CAR = {CAR.HYUNDAI_GENESIS, CAR.IONIQ_EV_2020, CAR.IONIQ_EV_LTD, CAR.IONIQ_PHEV, CAR.IONIQ, CAR.KONA_EV, CAR.KIA_SORENTO, CAR.SONATA_LF, CAR.KIA_OPTIMA_G4, CAR.KIA_OPTIMA_G4_FL, CAR.VELOSTER, + CAR.GENESIS_G70, CAR.GENESIS_G80, CAR.KIA_CEED, CAR.ELANTRA, CAR.IONIQ_HEV_2022} # If 0x500 is present on bus 1 it probably has a Mando radar outputting radar points. # If no points are outputted by default it might be possible to turn it on using selfdrive/debug/hyundai_enable_radar_points.py @@ -1634,6 +1720,7 @@ DBC = { CAR.IONIQ_HEV_2022: dbc_dict('hyundai_kia_generic', None), CAR.KIA_FORTE: dbc_dict('hyundai_kia_generic', None), CAR.KIA_K5_2021: dbc_dict('hyundai_kia_generic', None), + CAR.KIA_K5_HEV_2020: dbc_dict('hyundai_kia_generic', 'hyundai_kia_mando_front_radar_generated'), CAR.KIA_NIRO_EV: dbc_dict('hyundai_kia_generic', 'hyundai_kia_mando_front_radar_generated'), CAR.KIA_NIRO_PHEV: dbc_dict('hyundai_kia_generic', 'hyundai_kia_mando_front_radar_generated'), CAR.KIA_NIRO_HEV_2021: dbc_dict('hyundai_kia_generic', None), @@ -1670,4 +1757,5 @@ DBC = { CAR.KIA_SORENTO_PHEV_4TH_GEN: dbc_dict('hyundai_canfd', None), CAR.GENESIS_GV60_EV_1ST_GEN: dbc_dict('hyundai_canfd', None), CAR.KIA_SORENTO_4TH_GEN: dbc_dict('hyundai_canfd', None), + CAR.KIA_NIRO_HEV_2ND_GEN: dbc_dict('hyundai_canfd', None), } diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index 7192f5252c..249818369c 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -233,7 +233,7 @@ class CarInterfaceBase(ABC): return reader @abstractmethod - def apply(self, c: car.CarControl) -> Tuple[car.CarControl.Actuators, List[bytes]]: + def apply(self, c: car.CarControl, now_nanos: int) -> Tuple[car.CarControl.Actuators, List[bytes]]: pass def create_common_events(self, cs_out, extra_gears=None, pcm_enable=True, allow_enable=True, @@ -401,15 +401,15 @@ class CarStateBase(ABC): return GearShifter.unknown d: Dict[str, car.CarState.GearShifter] = { - 'P': GearShifter.park, 'PARK': GearShifter.park, - 'R': GearShifter.reverse, 'REVERSE': GearShifter.reverse, - 'N': GearShifter.neutral, 'NEUTRAL': GearShifter.neutral, - 'E': GearShifter.eco, 'ECO': GearShifter.eco, - 'T': GearShifter.manumatic, 'MANUAL': GearShifter.manumatic, - 'D': GearShifter.drive, 'DRIVE': GearShifter.drive, - 'S': GearShifter.sport, 'SPORT': GearShifter.sport, - 'L': GearShifter.low, 'LOW': GearShifter.low, - 'B': GearShifter.brake, 'BRAKE': GearShifter.brake, + 'P': GearShifter.park, 'PARK': GearShifter.park, + 'R': GearShifter.reverse, 'REVERSE': GearShifter.reverse, + 'N': GearShifter.neutral, 'NEUTRAL': GearShifter.neutral, + 'E': GearShifter.eco, 'ECO': GearShifter.eco, + 'T': GearShifter.manumatic, 'MANUAL': GearShifter.manumatic, + 'D': GearShifter.drive, 'DRIVE': GearShifter.drive, + 'S': GearShifter.sport, 'SPORT': GearShifter.sport, + 'L': GearShifter.low, 'LOW': GearShifter.low, + 'B': GearShifter.brake, 'BRAKE': GearShifter.brake, } return d.get(gear.upper(), GearShifter.unknown) diff --git a/selfdrive/car/mazda/carcontroller.py b/selfdrive/car/mazda/carcontroller.py index 027822cc3f..524a02a370 100644 --- a/selfdrive/car/mazda/carcontroller.py +++ b/selfdrive/car/mazda/carcontroller.py @@ -15,7 +15,7 @@ class CarController: self.brake_counter = 0 self.frame = 0 - def update(self, CC, CS): + def update(self, CC, CS, now_nanos): can_sends = [] apply_steer = 0 diff --git a/selfdrive/car/mazda/interface.py b/selfdrive/car/mazda/interface.py index fdd2439ff9..2930b002d4 100755 --- a/selfdrive/car/mazda/interface.py +++ b/selfdrive/car/mazda/interface.py @@ -14,7 +14,7 @@ class CarInterface(CarInterfaceBase): def _get_params(ret, candidate, fingerprint, car_fw, experimental_long): ret.carName = "mazda" ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.mazda)] - ret.radarOffCan = True + ret.radarUnavailable = True ret.dashcamOnly = candidate not in (CAR.CX5_2022, CAR.CX9_2021) @@ -69,5 +69,5 @@ class CarInterface(CarInterfaceBase): return ret - def apply(self, c): - return self.CC.update(c, self.CS) + def apply(self, c, now_nanos): + return self.CC.update(c, self.CS, now_nanos) diff --git a/selfdrive/car/mock/interface.py b/selfdrive/car/mock/interface.py index 3ac487dbb7..13210c86d5 100755 --- a/selfdrive/car/mock/interface.py +++ b/selfdrive/car/mock/interface.py @@ -57,7 +57,7 @@ class CarInterface(CarInterfaceBase): return ret - def apply(self, c): + def apply(self, c, now_nanos): # in mock no carcontrols actuators = car.CarControl.Actuators.new_message() return actuators, [] diff --git a/selfdrive/car/nissan/carcontroller.py b/selfdrive/car/nissan/carcontroller.py index ff13812398..45c3dd720c 100644 --- a/selfdrive/car/nissan/carcontroller.py +++ b/selfdrive/car/nissan/carcontroller.py @@ -18,7 +18,7 @@ class CarController: self.packer = CANPacker(dbc_name) - def update(self, CC, CS): + def update(self, CC, CS, now_nanos): actuators = CC.actuators hud_control = CC.hudControl pcm_cancel_cmd = CC.cruiseControl.cancel diff --git a/selfdrive/car/nissan/interface.py b/selfdrive/car/nissan/interface.py index 386e859089..074cd1cc57 100644 --- a/selfdrive/car/nissan/interface.py +++ b/selfdrive/car/nissan/interface.py @@ -19,7 +19,7 @@ class CarInterface(CarInterfaceBase): ret.steerRatio = 17 ret.steerControlType = car.CarParams.SteerControlType.angle - ret.radarOffCan = True + ret.radarUnavailable = True if candidate in (CAR.ROGUE, CAR.XTRAIL): ret.mass = 1610 + STD_CARGO_KG @@ -56,5 +56,5 @@ class CarInterface(CarInterfaceBase): return ret - def apply(self, c): - return self.CC.update(c, self.CS) + def apply(self, c, now_nanos): + return self.CC.update(c, self.CS, now_nanos) diff --git a/selfdrive/car/subaru/carcontroller.py b/selfdrive/car/subaru/carcontroller.py index a56e63408e..24d85877d7 100644 --- a/selfdrive/car/subaru/carcontroller.py +++ b/selfdrive/car/subaru/carcontroller.py @@ -19,7 +19,7 @@ class CarController: self.p = CarControllerParams(CP) self.packer = CANPacker(DBC[CP.carFingerprint]['pt']) - def update(self, CC, CS): + def update(self, CC, CS, now_nanos): actuators = CC.actuators hud_control = CC.hudControl pcm_cancel_cmd = CC.cruiseControl.cancel diff --git a/selfdrive/car/subaru/interface.py b/selfdrive/car/subaru/interface.py index 22468801ec..733482ef82 100644 --- a/selfdrive/car/subaru/interface.py +++ b/selfdrive/car/subaru/interface.py @@ -11,7 +11,7 @@ class CarInterface(CarInterfaceBase): @staticmethod def _get_params(ret, candidate, fingerprint, car_fw, experimental_long): ret.carName = "subaru" - ret.radarOffCan = True + ret.radarUnavailable = True ret.dashcamOnly = candidate in PREGLOBAL_CARS ret.autoResumeSng = False @@ -112,5 +112,5 @@ class CarInterface(CarInterfaceBase): return ret - def apply(self, c): - return self.CC.update(c, self.CS) + def apply(self, c, now_nanos): + return self.CC.update(c, self.CS, now_nanos) diff --git a/selfdrive/car/subaru/values.py b/selfdrive/car/subaru/values.py index 6ac2637fa2..5c7d3f5e4d 100644 --- a/selfdrive/car/subaru/values.py +++ b/selfdrive/car/subaru/values.py @@ -253,6 +253,7 @@ FW_VERSIONS = { b'\xca!f@\x07', b'\xca!fp\x07', b'\xf3"f@\x07', + b'\xe6!fp\x07', ], (Ecu.transmission, 0x7e1, None): [ b'\xe6\xf5\004\000\000', @@ -262,6 +263,7 @@ FW_VERSIONS = { b'\xf1\x00\xd7\x10@', b'\xe6\xf5D0\x00', b'\xe9\xf6F0\x00', + b'\xe9\xf5B0\x00', ], }, CAR.FORESTER: { diff --git a/selfdrive/car/tesla/carcontroller.py b/selfdrive/car/tesla/carcontroller.py index 6e2869d1c2..1f18c3d0ea 100644 --- a/selfdrive/car/tesla/carcontroller.py +++ b/selfdrive/car/tesla/carcontroller.py @@ -14,7 +14,7 @@ class CarController: self.pt_packer = CANPacker(DBC[CP.carFingerprint]['pt']) self.tesla_can = TeslaCAN(self.packer, self.pt_packer) - def update(self, CC, CS): + def update(self, CC, CS, now_nanos): actuators = CC.actuators pcm_cancel_cmd = CC.cruiseControl.cancel diff --git a/selfdrive/car/tesla/interface.py b/selfdrive/car/tesla/interface.py index 6a73472108..70d49896cb 100755 --- a/selfdrive/car/tesla/interface.py +++ b/selfdrive/car/tesla/interface.py @@ -58,5 +58,5 @@ class CarInterface(CarInterfaceBase): return ret - def apply(self, c): - return self.CC.update(c, self.CS) + def apply(self, c, now_nanos): + return self.CC.update(c, self.CS, now_nanos) diff --git a/selfdrive/car/tests/routes.py b/selfdrive/car/tests/routes.py index ac0521cc68..a4d2f55581 100644 --- a/selfdrive/car/tests/routes.py +++ b/selfdrive/car/tests/routes.py @@ -18,6 +18,7 @@ from selfdrive.car.body.values import CAR as COMMA non_tested_cars = [ FORD.ESCAPE_MK4, FORD.FOCUS_MK4, + FORD.MAVERICK_MK1, GM.CADILLAC_ATS, GM.HOLDEN_ASTRA, GM.MALIBU, @@ -25,6 +26,7 @@ non_tested_cars = [ HYUNDAI.GENESIS_G90, HYUNDAI.KIA_OPTIMA_H, HONDA.ODYSSEY_CHN, + VOLKSWAGEN.CRAFTER_MK2, # need a route from an ACC-equipped Crafter ] CarTestRoute = namedtuple('CarTestRoute', ['route', 'car_model', 'segment'], defaults=(None,)) @@ -42,6 +44,7 @@ routes = [ CarTestRoute("221c253375af4ee9|2022-06-15--18-38-24", CHRYSLER.RAM_1500), CarTestRoute("8fb5eabf914632ae|2022-08-04--17-28-53", CHRYSLER.RAM_HD, segment=6), + CarTestRoute("54827bf84c38b14f|2023-01-25--14-14-11", FORD.BRONCO_SPORT_MK1), CarTestRoute("62241b0c7fea4589|2022-09-01--15-32-49", FORD.EXPLORER_MK6), #TestRoute("f1b4c567731f4a1b|2018-04-30--10-15-35", FORD.FUSION), @@ -72,7 +75,7 @@ routes = [ CarTestRoute("d83f36766f8012a5|2020-02-05--18-42-21", HONDA.CIVIC_BOSCH_DIESEL), CarTestRoute("f0890d16a07a236b|2021-05-25--17-27-22", HONDA.INSIGHT), CarTestRoute("07d37d27996096b6|2020-03-04--21-57-27", HONDA.PILOT), - CarTestRoute("684e8f96bd491a0e|2021-11-03--11-08-42", HONDA.PASSPORT), + CarTestRoute("684e8f96bd491a0e|2021-11-03--11-08-42", HONDA.PILOT), # Passport CarTestRoute("0a78dfbacc8504ef|2020-03-04--13-29-55", HONDA.CIVIC_BOSCH), CarTestRoute("f34a60d68d83b1e5|2020-10-06--14-35-55", HONDA.ACURA_RDX), CarTestRoute("54fd8451b3974762|2021-04-01--14-50-10", HONDA.RIDGELINE), @@ -120,9 +123,11 @@ routes = [ CarTestRoute("d545129f3ca90f28|2022-10-19--09-22-54", HYUNDAI.KIA_EV6), # HDA2 CarTestRoute("68d6a96e703c00c9|2022-09-10--16-09-39", HYUNDAI.KIA_EV6), # HDA1 CarTestRoute("007d5e4ad9f86d13|2021-09-30--15-09-23", HYUNDAI.KIA_K5_2021), + CarTestRoute("c58dfc9fc16590e0|2023-01-14--13-51-48", HYUNDAI.KIA_K5_HEV_2020), CarTestRoute("50c6c9b85fd1ff03|2020-10-26--17-56-06", HYUNDAI.KIA_NIRO_EV), CarTestRoute("173219cf50acdd7b|2021-07-05--10-27-41", HYUNDAI.KIA_NIRO_PHEV), CarTestRoute("34a875f29f69841a|2021-07-29--13-02-09", HYUNDAI.KIA_NIRO_HEV_2021), + CarTestRoute("db04d2c63990e3ba|2023-02-08--16-52-39", HYUNDAI.KIA_NIRO_HEV_2ND_GEN), CarTestRoute("50a2212c41f65c7b|2021-05-24--16-22-06", HYUNDAI.KIA_FORTE), CarTestRoute("192283cdbb7a58c2|2022-10-15--01-43-18", HYUNDAI.KIA_SPORTAGE_5TH_GEN), CarTestRoute("c5ac319aa9583f83|2021-06-01--18-18-31", HYUNDAI.ELANTRA), @@ -175,7 +180,9 @@ routes = [ CarTestRoute("0a0de17a1e6a2d15|2020-09-21--21-24-41", TOYOTA.PRIUS_TSS2), CarTestRoute("9b36accae406390e|2021-03-30--10-41-38", TOYOTA.MIRAI), CarTestRoute("cd9cff4b0b26c435|2021-05-13--15-12-39", TOYOTA.CHR), + CarTestRoute("ea8fbe72b96a185c|2023-02-08--15-11-46", TOYOTA.CHR_TSS2), CarTestRoute("57858ede0369a261|2021-05-18--20-34-20", TOYOTA.CHRH), + CarTestRoute("6719965b0e1d1737|2023-02-09--22-44-05", TOYOTA.CHRH_TSS2), CarTestRoute("14623aae37e549f3|2021-10-24--01-20-49", TOYOTA.PRIUS_V), CarTestRoute("202c40641158a6e5|2021-09-21--09-43-24", VOLKSWAGEN.ARTEON_MK1), diff --git a/selfdrive/car/tests/test_car_interfaces.py b/selfdrive/car/tests/test_car_interfaces.py index deb0454b9c..78ecbe425e 100755 --- a/selfdrive/car/tests/test_car_interfaces.py +++ b/selfdrive/car/tests/test_car_interfaces.py @@ -59,15 +59,15 @@ class TestCarInterfaces(unittest.TestCase): CC = car.CarControl.new_message() for _ in range(10): car_interface.update(CC, []) - car_interface.apply(CC) - car_interface.apply(CC) + car_interface.apply(CC, 0) + car_interface.apply(CC, 0) CC = car.CarControl.new_message() CC.enabled = True for _ in range(10): car_interface.update(CC, []) - car_interface.apply(CC) - car_interface.apply(CC) + car_interface.apply(CC, 0) + car_interface.apply(CC, 0) # Test radar interface RadarInterface = importlib.import_module(f'selfdrive.car.{car_params.carName}.radar_interface').RadarInterface @@ -76,7 +76,7 @@ class TestCarInterfaces(unittest.TestCase): # Run radar interface once radar_interface.update([]) - if not car_params.radarOffCan and radar_interface.rcp is not None and \ + if not car_params.radarUnavailable and radar_interface.rcp is not None and \ hasattr(radar_interface, '_update') and hasattr(radar_interface, 'trigger_msg'): radar_interface._update([radar_interface.trigger_msg]) diff --git a/selfdrive/car/tests/test_models.py b/selfdrive/car/tests/test_models.py index b6f78c4b47..6fbe1436f1 100755 --- a/selfdrive/car/tests/test_models.py +++ b/selfdrive/car/tests/test_models.py @@ -149,7 +149,7 @@ class TestCarModelBase(unittest.TestCase): for i, msg in enumerate(self.can_msgs): CS = self.CI.update(CC, (msg.as_builder().to_bytes(),)) - self.CI.apply(CC) + self.CI.apply(CC, msg.logMonoTime) if CS.canValid: can_valid = True @@ -245,7 +245,7 @@ class TestCarModelBase(unittest.TestCase): # 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: + if self.CP.carFingerprint in (HONDA.PILOT, HONDA.RIDGELINE) and CS.brake > 0.05: brake_pressed = False checks['brakePressed'] += brake_pressed != self.safety.get_brake_pressed_prev() checks['regenBraking'] += CS.regenBraking != self.safety.get_regen_braking_prev() diff --git a/selfdrive/car/torque_data/override.yaml b/selfdrive/car/torque_data/override.yaml index 3a9f92c046..52c9e8d547 100644 --- a/selfdrive/car/torque_data/override.yaml +++ b/selfdrive/car/torque_data/override.yaml @@ -12,9 +12,11 @@ TESLA AP1 MODEL S: [.nan, 2.5, .nan] TESLA AP2 MODEL S: [.nan, 2.5, .nan] # Guess +FORD BRONCO SPORT 1ST GEN: [.nan, 1.5, .nan] FORD ESCAPE 4TH GEN: [.nan, 1.5, .nan] FORD EXPLORER 6TH GEN: [.nan, 1.5, .nan] FORD FOCUS 4TH GEN: [.nan, 1.5, .nan] +FORD MAVERICK 1ST GEN: [.nan, 1.5, .nan] ### # No steering wheel @@ -24,7 +26,6 @@ COMMA BODY: [.nan, 1000, .nan] RAM 1500 5TH GEN: [2.0, 2.0, 0.0] RAM HD 5TH GEN: [1.4, 1.4, 0.0] SUBARU OUTBACK 6TH GEN: [2.3, 2.3, 0.11] -CHEVROLET BOLT EV 2022: [2.0, 2.0, 0.05] CHEVROLET BOLT EUV 2022: [2.0, 2.0, 0.05] CHEVROLET SILVERADO 1500 2020: [1.9, 1.9, 0.112] CHEVROLET EQUINOX 2019: [2.0, 2.0, 0.05] @@ -37,6 +38,7 @@ GENESIS GV70 1ST GEN: [2.42, 2.42, 0.1] KIA SORENTO PLUG-IN HYBRID 4TH GEN: [2.5, 2.5, 0.1] GENESIS GV60 ELECTRIC 1ST GEN: [2.5, 2.5, 0.1] KIA SORENTO 4TH GEN: [2.5, 2.5, 0.1] +KIA NIRO HYBRID 2ND GEN: [2.42, 2.5, 0.12] # Dashcam or fallback configured as ideal car mock: [10.0, 10, 0.0] diff --git a/selfdrive/car/torque_data/params.yaml b/selfdrive/car/torque_data/params.yaml index eb1a04cee6..6f8cfe0ce6 100644 --- a/selfdrive/car/torque_data/params.yaml +++ b/selfdrive/car/torque_data/params.yaml @@ -4,11 +4,11 @@ ACURA RDX 2020: [1.4314459806646749, 0.33874701282109954, 0.18048847083897598] AUDI A3 3RD GEN: [1.5122414863077502, 1.7443517531719404, 0.15194151892450905] AUDI Q3 2ND GEN: [1.4439223359448605, 1.2254955789112076, 0.1413798895978097] CHEVROLET VOLT PREMIER 2017: [1.5961527626411784, 1.8422651988094612, 0.1572393918005158] -CHRYSLER PACIFICA 2018: [1.593387270257916, 1.3366521181047952, 0.13776367250652022] -CHRYSLER PACIFICA 2020: [1.4323553627965695, 1.509076559398423, 0.14328246159386085] -CHRYSLER PACIFICA HYBRID 2017: [1.3032470208409048, 1.06831764583744, 0.13287170990024627] -CHRYSLER PACIFICA HYBRID 2018: [1.6068280248761635, 1.2943025830995154, 0.1358557824293823] -CHRYSLER PACIFICA HYBRID 2019: [1.4624643614072217, 1.1958788168371808, 0.15748488008472716] +CHRYSLER PACIFICA 2018: [2.07140, 1.3366521181047952, 0.13776367250652022] +CHRYSLER PACIFICA 2020: [1.86206, 1.509076559398423, 0.14328246159386085] +CHRYSLER PACIFICA HYBRID 2017: [1.79422, 1.06831764583744, 0.116237] +CHRYSLER PACIFICA HYBRID 2018: [2.08887, 1.2943025830995154, 0.114818] +CHRYSLER PACIFICA HYBRID 2019: [1.90120, 1.1958788168371808, 0.131520] GENESIS G70 2018: [3.8520195946707947, 2.354697063349854, 0.06830285485626221] GMC ACADIA DENALI 2018: [1.3181430320331884, 1.1853735340610179, 0.3450592280031644] HONDA ACCORD 2018: [1.7135052593468778, 0.3461280068322071, 0.21579936052863807] @@ -22,11 +22,10 @@ HONDA FIT 2018: [1.5719981427109775, 0.5712761407108976, 0.110773383324281] HONDA HRV 2019: [2.0661212805710205, 0.7521343418694775, 0.17760375789242094] HONDA INSIGHT 2019: [1.5201671214069354, 0.5660229120683284, 0.25808042580281876] HONDA ODYSSEY 2018: [1.8774809275211801, 0.8394431662987996, 0.2096978613792822] -HONDA PASSPORT 2021: [1.5305538930036766, 0.7956063674638759, 0.19599407381531284] HONDA PILOT 2017: [1.7262026201812795, 0.9470005614967523, 0.21351430733218763] HONDA RIDGELINE 2017: [1.4146525028237624, 0.7356572861629564, 0.23307177552211328] HYUNDAI ELANTRA 2021: [3.169, 2.1259108157250735, 0.0819] -HYUNDAI GENESIS 2015-2016: [1.8466226943929824, 1.5552063647830634, 0.0984484465421171] +HYUNDAI GENESIS 2015-2016: [2.7807965280270794, 2.325, 0.0984484465421171] HYUNDAI IONIQ 5 2022: [3.172929, 2.713050, 0.096019] HYUNDAI IONIQ ELECTRIC LIMITED 2019: [1.7662975472852054, 1.613755614526594, 0.17087579756306276] HYUNDAI IONIQ PHEV 2020: [3.2928700076638537, 2.1193482926455656, 0.12463700961468778] @@ -40,8 +39,8 @@ HYUNDAI SONATA 2019: [2.2200457811703953, 1.2967330275895228, 0.1403992098658639 HYUNDAI SONATA 2020: [2.9638737459977467, 2.1259108157250735, 0.07813665616927593] HYUNDAI SONATA HYBRID 2021: [2.8990264092395734, 2.061410192222139, 0.0899805488717382] HYUNDAI TUCSON HYBRID 4TH GEN: [2.035545, 2.035545, 0.110272] -JEEP GRAND CHEROKEE 2019: [1.7321233388827006, 1.289689569171081, 0.15046331002097185] -JEEP GRAND CHEROKEE V6 2018: [1.8776598027756923, 1.4057367824262523, 0.11725947414922003] +JEEP GRAND CHEROKEE 2019: [2.30972, 1.289689569171081, 0.117048] +JEEP GRAND CHEROKEE V6 2018: [2.27116, 1.4057367824262523, 0.11725947414922003] KIA EV6 2022: [3.2, 2.093457, 0.05] KIA K5 2021: [2.405339728085138, 1.460032270828705, 0.11650989850813716] KIA NIRO EV 2020: [2.9215954981365337, 2.1500583840260044, 0.09236802474810267] diff --git a/selfdrive/car/torque_data/substitute.yaml b/selfdrive/car/torque_data/substitute.yaml index aeb2e6f280..61243424f0 100644 --- a/selfdrive/car/torque_data/substitute.yaml +++ b/selfdrive/car/torque_data/substitute.yaml @@ -9,6 +9,7 @@ TOYOTA ALPHARD 2020: TOYOTA SIENNA 2018 TOYOTA PRIUS v 2017 : TOYOTA PRIUS 2017 TOYOTA RAV4 2022: TOYOTA RAV4 HYBRID 2022 TOYOTA C-HR HYBRID 2018: TOYOTA C-HR 2018 +TOYOTA C-HR HYBRID 2022: TOYOTA C-HR 2021 LEXUS IS 2018: LEXUS NX 2018 LEXUS CT HYBRID 2018 : LEXUS NX 2018 LEXUS ES HYBRID 2018: TOYOTA CAMRY HYBRID 2018 @@ -37,6 +38,7 @@ HYUNDAI ELANTRA HYBRID 2021: HYUNDAI SONATA 2020 HYUNDAI TUCSON 2019: HYUNDAI SANTA FE 2019 HYUNDAI TUCSON 4TH GEN: HYUNDAI TUCSON HYBRID 4TH GEN HYUNDAI SANTA FE 2022: HYUNDAI SANTA FE HYBRID 2022 +KIA K5 HYBRID 2020: KIA K5 2021 KIA STINGER 2022: KIA STINGER GT2 2018 GENESIS G90 2017: GENESIS G70 2018 GENESIS G80 2017: GENESIS G70 2018 @@ -59,6 +61,7 @@ SKODA SCALA 1ST GEN: SKODA SUPERB 3RD GEN SKODA KODIAQ 1ST GEN: SKODA SUPERB 3RD GEN SKODA KAROQ 1ST GEN: SKODA SUPERB 3RD GEN SKODA KAMIQ 1ST GEN: SKODA SUPERB 3RD GEN +VOLKSWAGEN CRAFTER 2ND GEN: VOLKSWAGEN TIGUAN 2ND GEN VOLKSWAGEN T-ROC 1ST GEN: VOLKSWAGEN TIGUAN 2ND GEN VOLKSWAGEN T-CROSS 1ST GEN: VOLKSWAGEN TIGUAN 2ND GEN VOLKSWAGEN TOURAN 2ND GEN: VOLKSWAGEN TIGUAN 2ND GEN diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index 6dbdd4b5d9..2e0d7009c8 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -34,7 +34,7 @@ class CarController: self.gas = 0 self.accel = 0 - def update(self, CC, CS): + def update(self, CC, CS, now_nanos): actuators = CC.actuators hud_control = CC.hudControl pcm_cancel_cmd = CC.cruiseControl.cancel @@ -143,7 +143,7 @@ class CarController: # forcing the pcm to disengage causes a bad fault sound so play a good sound instead send_ui = True - if self.frame % 100 == 0 or send_ui: + if self.frame % 20 == 0 or send_ui: can_sends.append(create_ui_command(self.packer, steer_alert, pcm_cancel_cmd, hud_control.leftLaneVisible, hud_control.rightLaneVisible, hud_control.leftLaneDepart, hud_control.rightLaneDepart, CC.enabled, CS.lkas_hud)) diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index 8e180e2301..8b3fd048d9 100644 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -72,7 +72,7 @@ class CarInterface(CarInterfaceBase): tire_stiffness_factor = 0.5533 ret.mass = 4481. * CV.LB_TO_KG + STD_CARGO_KG # mean between min and max - elif candidate in (CAR.CHR, CAR.CHRH): + elif candidate in (CAR.CHR, CAR.CHRH, CAR.CHR_TSS2, CAR.CHRH_TSS2): stop_and_go = True ret.wheelbase = 2.63906 ret.steerRatio = 13.6 @@ -263,5 +263,5 @@ class CarInterface(CarInterfaceBase): # pass in a car.CarControl # to be called @ 100hz - def apply(self, c): - return self.CC.update(c, self.CS) + def apply(self, c, now_nanos): + return self.CC.update(c, self.CS, now_nanos) diff --git a/selfdrive/car/toyota/values.py b/selfdrive/car/toyota/values.py index c413028a25..b6a556438d 100644 --- a/selfdrive/car/toyota/values.py +++ b/selfdrive/car/toyota/values.py @@ -49,7 +49,9 @@ class CAR: CAMRY_TSS2 = "TOYOTA CAMRY 2021" # TSS 2.5 CAMRYH_TSS2 = "TOYOTA CAMRY HYBRID 2021" CHR = "TOYOTA C-HR 2018" + CHR_TSS2 = "TOYOTA C-HR 2021" CHRH = "TOYOTA C-HR HYBRID 2018" + CHRH_TSS2 = "TOYOTA C-HR HYBRID 2022" COROLLA = "TOYOTA COROLLA 2017" COROLLA_TSS2 = "TOYOTA COROLLA TSS2 2019" # LSS2 Lexus UX Hybrid is same as a TSS2 Corolla Hybrid @@ -115,8 +117,10 @@ CAR_INFO: Dict[str, Union[ToyotaCarInfo, List[ToyotaCarInfo]]] = { CAR.CAMRYH: ToyotaCarInfo("Toyota Camry Hybrid 2018-20", video_link="https://www.youtube.com/watch?v=Q2DYY0AWKgk"), CAR.CAMRY_TSS2: ToyotaCarInfo("Toyota Camry 2021-22", footnotes=[Footnote.CAMRY]), CAR.CAMRYH_TSS2: ToyotaCarInfo("Toyota Camry Hybrid 2021-23"), - CAR.CHR: ToyotaCarInfo("Toyota C-HR 2017-21"), + CAR.CHR: ToyotaCarInfo("Toyota C-HR 2017-20"), + CAR.CHR_TSS2: ToyotaCarInfo("Toyota C-HR 2021"), CAR.CHRH: ToyotaCarInfo("Toyota C-HR Hybrid 2017-19"), + CAR.CHRH_TSS2: ToyotaCarInfo("Toyota C-HR Hybrid 2022"), CAR.COROLLA: ToyotaCarInfo("Toyota Corolla 2017-19"), CAR.COROLLA_TSS2: [ ToyotaCarInfo("Toyota Corolla 2020-22", video_link="https://www.youtube.com/watch?v=_66pXk0CBYA"), @@ -125,6 +129,7 @@ CAR_INFO: Dict[str, Union[ToyotaCarInfo, List[ToyotaCarInfo]]] = { ], CAR.COROLLAH_TSS2: [ ToyotaCarInfo("Toyota Corolla Hybrid 2020-22"), + ToyotaCarInfo("Toyota Corolla Hybrid (Non-US only) 2020-23", min_enable_speed=7.5), ToyotaCarInfo("Toyota Corolla Cross Hybrid (Non-US only) 2020-22", min_enable_speed=7.5), ToyotaCarInfo("Lexus UX Hybrid 2019-22"), ], @@ -161,7 +166,7 @@ CAR_INFO: Dict[str, Union[ToyotaCarInfo, List[ToyotaCarInfo]]] = { CAR.LEXUS_CTH: ToyotaCarInfo("Lexus CT Hybrid 2017-18", "Lexus Safety System+"), CAR.LEXUS_ESH: ToyotaCarInfo("Lexus ES Hybrid 2017-18", "Lexus Safety System+"), CAR.LEXUS_ES_TSS2: ToyotaCarInfo("Lexus ES 2019-22"), - CAR.LEXUS_ESH_TSS2: ToyotaCarInfo("Lexus ES Hybrid 2019-22", video_link="https://youtu.be/BZ29osRVJeg?t=12"), + CAR.LEXUS_ESH_TSS2: ToyotaCarInfo("Lexus ES Hybrid 2019-23", video_link="https://youtu.be/BZ29osRVJeg?t=12"), CAR.LEXUS_IS: ToyotaCarInfo("Lexus IS 2017-19"), CAR.LEXUS_NX: ToyotaCarInfo("Lexus NX 2018-19"), CAR.LEXUS_NXH: ToyotaCarInfo("Lexus NX Hybrid 2018-19"), @@ -230,7 +235,7 @@ FW_QUERY_CONFIG = FwQueryConfig( # FIXME: On some models, abs can sometimes be missing Ecu.abs: [CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.SIENNA, CAR.LEXUS_IS], # On some models, the engine can show on two different addresses - Ecu.engine: [CAR.CAMRY, CAR.COROLLA_TSS2, CAR.CHR, CAR.LEXUS_IS, CAR.LEXUS_RC], + Ecu.engine: [CAR.CAMRY, CAR.COROLLA_TSS2, CAR.CHR, CAR.CHR_TSS2, CAR.LEXUS_IS, CAR.LEXUS_RC], } ) @@ -347,6 +352,7 @@ FW_VERSIONS = { ], (Ecu.fwdRadar, 0x750, 0xf): [ b'\x018821F6201200\x00\x00\x00\x00', + b'\x018821F6201300\x00\x00\x00\x00', ], (Ecu.fwdCamera, 0x750, 0x6d): [ b'\x028646F4104100\x00\x00\x00\x008646G5301200\x00\x00\x00\x00', @@ -631,6 +637,30 @@ FW_VERSIONS = { b'8646FF407000 ', ], }, + CAR.CHR_TSS2: { + (Ecu.abs, 0x7b0, None): [ + b'F152610260\x00\x00\x00\x00\x00\x00', + b'F1526F4270\x00\x00\x00\x00\x00\x00', + ], + (Ecu.eps, 0x7a1, None): [ + b'8965B10091\x00\x00\x00\x00\x00\x00', + b'8965B10110\x00\x00\x00\x00\x00\x00', + ], + (Ecu.engine, 0x700, None): [ + b'\x0189663F459000\x00\x00\x00\x00', + ], + (Ecu.engine, 0x7e0, None): [ + b'\x0331014000\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00895231203402\x00\x00\x00\x00', + ], + (Ecu.fwdRadar, 0x750, 0xf): [ + b'\x018821FF410200\x00\x00\x00\x00', + b'\x018821FF410300\x00\x00\x00\x00', + ], + (Ecu.fwdCamera, 0x750, 0x6d): [ + b'\x028646FF410200\x00\x00\x00\x008646GF408200\x00\x00\x00\x00', + b'\x028646FF411100\x00\x00\x00\x008646GF409000\x00\x00\x00\x00', + ], + }, CAR.CHRH: { (Ecu.engine, 0x700, None): [ b'\x0289663F405100\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', @@ -684,6 +714,23 @@ FW_VERSIONS = { b'8646FF407000 ', ], }, + CAR.CHRH_TSS2: { + (Ecu.eps, 0x7a1, None): [ + b'8965B10092\x00\x00\x00\x00\x00\x00', + ], + (Ecu.abs, 0x7b0, None): [ + b'F152610041\x00\x00\x00\x00\x00\x00', + ], + (Ecu.engine, 0x700, None): [ + b'\x0189663F438000\x00\x00\x00\x00', + ], + (Ecu.fwdRadar, 0x750, 15): [ + b'\x018821FF410500\x00\x00\x00\x00', + ], + (Ecu.fwdCamera, 0x750, 109): [ + b'\x028646FF413100\x00\x00\x00\x008646GF411100\x00\x00\x00\x00', + ], + }, CAR.COROLLA: { (Ecu.engine, 0x7e0, None): [ b'\x0230ZC2000\x00\x00\x00\x00\x00\x00\x00\x0050212000\x00\x00\x00\x00\x00\x00\x00\x00', @@ -841,11 +888,13 @@ FW_VERSIONS = { b'\x02896630A07000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', b'\x02896630A21000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', b'\x02896630ZJ5000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', + b'\x02896630ZK8000\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', b'\x02896630ZR2000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', b'\x02896630ZT8000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', b'\x02896630ZT9000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', + b'\x02896630ZZ0000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', b'\x028966312K6000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', b'\x028966312L0000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', b'\x028966312Q3000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', @@ -858,6 +907,7 @@ FW_VERSIONS = { b'8965B12361\x00\x00\x00\x00\x00\x00', b'8965B12451\x00\x00\x00\x00\x00\x00', b'8965B16011\x00\x00\x00\x00\x00\x00', + b'8965B16101\x00\x00\x00\x00\x00\x00', b'8965B76012\x00\x00\x00\x00\x00\x00', b'8965B76050\x00\x00\x00\x00\x00\x00', b'\x018965B12350\x00\x00\x00\x00\x00\x00', @@ -905,6 +955,7 @@ FW_VERSIONS = { b'\x028646F1202100\x00\x00\x00\x008646G2601400\x00\x00\x00\x00', b'\x028646F1202200\x00\x00\x00\x008646G2601500\x00\x00\x00\x00', b'\x028646F1601100\x00\x00\x00\x008646G2601400\x00\x00\x00\x00', + b'\x028646F1601200\x00\x00\x00\x008646G2601400\x00\x00\x00\x00', b"\x028646F1601300\x00\x00\x00\x008646G2601400\x00\x00\x00\x00", b'\x028646F4203400\x00\x00\x00\x008646G2601200\x00\x00\x00\x00', b'\x028646F76020C0\x00\x00\x00\x008646G26011A0\x00\x00\x00\x00', @@ -1007,6 +1058,7 @@ FW_VERSIONS = { b'\x01896630ED9100\x00\x00\x00\x00', b'\x01896630EE1000\x00\x00\x00\x00', b'\x01896630EE1100\x00\x00\x00\x00', + b'\x01896630EG3000\x00\x00\x00\x00', b'\x01896630EG5000\x00\x00\x00\x00', ], (Ecu.fwdRadar, 0x750, 0xf): [ @@ -1586,12 +1638,14 @@ FW_VERSIONS = { b'\x028966333V4000\x00\x00\x00\x00897CF3305001\x00\x00\x00\x00', b'\x02896633T09000\x00\x00\x00\x00897CF3307001\x00\x00\x00\x00', b'\x01896633T38000\x00\x00\x00\x00', + b'\x01896633T58000\x00\x00\x00\x00', ], (Ecu.abs, 0x7b0, None): [ b'F152633423\x00\x00\x00\x00\x00\x00', b'F152633680\x00\x00\x00\x00\x00\x00', b'F152633681\x00\x00\x00\x00\x00\x00', b'F152633F50\x00\x00\x00\x00\x00\x00', + b'F152633F51\x00\x00\x00\x00\x00\x00', ], (Ecu.eps, 0x7a1, None): [ b'8965B33252\x00\x00\x00\x00\x00\x00', @@ -1607,6 +1661,7 @@ FW_VERSIONS = { b'\x018821F6201300\x00\x00\x00\x00', ], (Ecu.fwdCamera, 0x750, 0x6d): [ + b'\x028646F0610000\x00\x00\x00\x008646G3304000\x00\x00\x00\x00', b'\x028646F33030D0\x00\x00\x00\x008646G26011A0\x00\x00\x00\x00', b'\x028646F3303100\x00\x00\x00\x008646G26011A0\x00\x00\x00\x00', b'\x028646F3303200\x00\x00\x00\x008646G26011A0\x00\x00\x00\x00', @@ -2020,7 +2075,9 @@ DBC = { CAR.LEXUS_RX_TSS2: dbc_dict('toyota_nodsu_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.CHR_TSS2: dbc_dict('toyota_nodsu_pt_generated', None), CAR.CHRH: dbc_dict('toyota_nodsu_pt_generated', 'toyota_adas'), + CAR.CHRH_TSS2: dbc_dict('toyota_nodsu_pt_generated', None), CAR.CAMRY: dbc_dict('toyota_nodsu_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'), @@ -2062,7 +2119,7 @@ EPS_SCALE = defaultdict(lambda: 73, {CAR.PRIUS: 66, CAR.COROLLA: 88, CAR.LEXUS_I # Toyota/Lexus Safety Sense 2.0 and 2.5 TSS2_CAR = {CAR.RAV4_TSS2, CAR.RAV4_TSS2_2022, CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2, CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2, CAR.RAV4H_TSS2, CAR.RAV4H_TSS2_2022, 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.LEXUS_NXH_TSS2, CAR.ALPHARD_TSS2, CAR.AVALON_TSS2, CAR.AVALONH_TSS2, CAR.ALPHARDH_TSS2} + CAR.MIRAI, CAR.LEXUS_NX_TSS2, CAR.LEXUS_NXH_TSS2, CAR.ALPHARD_TSS2, CAR.AVALON_TSS2, CAR.AVALONH_TSS2, CAR.ALPHARDH_TSS2, CAR.CHR_TSS2, CAR.CHRH_TSS2} NO_DSU_CAR = TSS2_CAR | {CAR.CHR, CAR.CHRH, CAR.CAMRY, CAR.CAMRYH} @@ -2070,9 +2127,9 @@ NO_DSU_CAR = TSS2_CAR | {CAR.CHR, CAR.CHRH, CAR.CAMRY, CAR.CAMRYH} UNSUPPORTED_DSU_CAR = {CAR.LEXUS_IS, CAR.LEXUS_RC} # these cars have a radar which sends ACC messages instead of the camera -RADAR_ACC_CAR = {CAR.RAV4H_TSS2_2022, CAR.RAV4_TSS2_2022} +RADAR_ACC_CAR = {CAR.RAV4H_TSS2_2022, CAR.RAV4_TSS2_2022, CAR.CHR_TSS2, CAR.CHRH_TSS2} -EV_HYBRID_CAR = {CAR.AVALONH_2019, CAR.AVALONH_TSS2, CAR.CAMRYH, CAR.CAMRYH_TSS2, CAR.CHRH, CAR.COROLLAH_TSS2, CAR.HIGHLANDERH, CAR.HIGHLANDERH_TSS2, CAR.PRIUS, +EV_HYBRID_CAR = {CAR.AVALONH_2019, CAR.AVALONH_TSS2, CAR.CAMRYH, CAR.CAMRYH_TSS2, CAR.CHRH, CAR.CHRH_TSS2, CAR.COROLLAH_TSS2, CAR.HIGHLANDERH, CAR.HIGHLANDERH_TSS2, CAR.PRIUS, CAR.PRIUS_V, CAR.RAV4H, CAR.RAV4H_TSS2, CAR.RAV4H_TSS2_2022, CAR.LEXUS_CTH, CAR.MIRAI, CAR.LEXUS_ESH, CAR.LEXUS_ESH_TSS2, CAR.LEXUS_NXH, CAR.LEXUS_RXH, CAR.LEXUS_RXH_TSS2, CAR.LEXUS_NXH_TSS2, CAR.PRIUS_TSS2, CAR.ALPHARDH_TSS2} diff --git a/selfdrive/car/volkswagen/carcontroller.py b/selfdrive/car/volkswagen/carcontroller.py index 4b19f4d13c..5d00b5a52f 100644 --- a/selfdrive/car/volkswagen/carcontroller.py +++ b/selfdrive/car/volkswagen/carcontroller.py @@ -24,7 +24,7 @@ class CarController: self.hcaSameTorqueCount = 0 self.hcaEnabledFrameCount = 0 - def update(self, CC, CS, ext_bus): + def update(self, CC, CS, ext_bus, now_nanos): actuators = CC.actuators hud_control = CC.hudControl can_sends = [] diff --git a/selfdrive/car/volkswagen/interface.py b/selfdrive/car/volkswagen/interface.py index da0ce25afa..521c68184d 100644 --- a/selfdrive/car/volkswagen/interface.py +++ b/selfdrive/car/volkswagen/interface.py @@ -23,7 +23,7 @@ class CarInterface(CarInterfaceBase): @staticmethod def _get_params(ret, candidate, fingerprint, car_fw, experimental_long): ret.carName = "volkswagen" - ret.radarOffCan = True + ret.radarUnavailable = True use_off_car_defaults = len(fingerprint[0]) == 0 # Pick sensible carParams during offline doc generation/CI jobs @@ -107,6 +107,11 @@ class CarInterface(CarInterfaceBase): ret.mass = 2011 + STD_CARGO_KG ret.wheelbase = 2.98 + elif candidate == CAR.CRAFTER_MK2: + ret.mass = 2100 + STD_CARGO_KG + ret.wheelbase = 3.64 # SWB, LWB is 4.49, TBD how to detect difference + ret.minSteerSpeed = 50 * CV.KPH_TO_MS + elif candidate == CAR.GOLF_MK7: ret.mass = 1397 + STD_CARGO_KG ret.wheelbase = 2.62 @@ -239,5 +244,5 @@ class CarInterface(CarInterfaceBase): return ret - def apply(self, c): - return self.CC.update(c, self.CS, self.ext_bus) + def apply(self, c, now_nanos): + return self.CC.update(c, self.CS, self.ext_bus, now_nanos) diff --git a/selfdrive/car/volkswagen/values.py b/selfdrive/car/volkswagen/values.py index f627e517be..cd30bef73b 100755 --- a/selfdrive/car/volkswagen/values.py +++ b/selfdrive/car/volkswagen/values.py @@ -111,6 +111,7 @@ class CANBUS: class CAR: ARTEON_MK1 = "VOLKSWAGEN ARTEON 1ST GEN" # Chassis AN, Mk1 VW Arteon and variants ATLAS_MK1 = "VOLKSWAGEN ATLAS 1ST GEN" # Chassis CA, Mk1 VW Atlas and Atlas Cross Sport + CRAFTER_MK2 = "VOLKSWAGEN CRAFTER 2ND GEN" # Chassis SY/SZ, Mk2 VW Crafter, VW Grand California, MAN TGE GOLF_MK7 = "VOLKSWAGEN GOLF 7TH GEN" # Chassis 5G/AU/BA/BE, Mk7 VW Golf and variants JETTA_MK7 = "VOLKSWAGEN JETTA 7TH GEN" # Chassis BU, Mk7 VW Jetta PASSAT_MK8 = "VOLKSWAGEN PASSAT 8TH GEN" # Chassis 3G, Mk8 VW Passat and variants @@ -122,7 +123,7 @@ class CAR: TIGUAN_MK2 = "VOLKSWAGEN TIGUAN 2ND GEN" # Chassis AD/BW, Mk2 VW Tiguan and variants TOURAN_MK2 = "VOLKSWAGEN TOURAN 2ND GEN" # Chassis 1T, Mk2 VW Touran and variants TRANSPORTER_T61 = "VOLKSWAGEN TRANSPORTER T6.1" # Chassis 7H/7L, T6-facelift Transporter/Multivan/Caravelle/California - TROC_MK1 = "VOLKSWAGEN T-ROC 1ST GEN" # Chassis A1, Mk1 VW VW T-Roc and variants + TROC_MK1 = "VOLKSWAGEN T-ROC 1ST GEN" # Chassis A1, Mk1 VW T-Roc and variants AUDI_A3_MK3 = "AUDI A3 3RD GEN" # Chassis 8V/FF, Mk3 Audi A3 and variants AUDI_Q2_MK1 = "AUDI Q2 1ST GEN" # Chassis GA, Mk1 Audi Q2 (RoW) and Q2L (China only) AUDI_Q3_MK2 = "AUDI Q3 2ND GEN" # Chassis 8U/F3/FS, Mk2 Audi Q3 and variants @@ -184,6 +185,13 @@ CAR_INFO: Dict[str, Union[VWCarInfo, List[VWCarInfo]]] = { VWCarInfo("Volkswagen Teramont Cross Sport 2021-22"), VWCarInfo("Volkswagen Teramont X 2021-22"), ], + CAR.CRAFTER_MK2: [ + VWCarInfo("Volkswagen Crafter 2017-23", video_link="https://youtu.be/4100gLeabmo"), + VWCarInfo("Volkswagen e-Crafter 2018-23", video_link="https://youtu.be/4100gLeabmo"), + VWCarInfo("Volkswagen Grand California 2019-23", video_link="https://youtu.be/4100gLeabmo"), + VWCarInfo("MAN TGE 2017-23", video_link="https://youtu.be/4100gLeabmo"), + VWCarInfo("MAN eTGE 2020-23", video_link="https://youtu.be/4100gLeabmo"), + ], CAR.GOLF_MK7: [ VWCarInfo("Volkswagen e-Golf 2014-20"), VWCarInfo("Volkswagen Golf 2015-20"), @@ -214,7 +222,7 @@ CAR_INFO: Dict[str, Union[VWCarInfo, List[VWCarInfo]]] = { ], CAR.TAOS_MK1: VWCarInfo("Volkswagen Taos 2022"), CAR.TCROSS_MK1: VWCarInfo("Volkswagen T-Cross 2021", footnotes=[Footnote.VW_MQB_A0]), - CAR.TIGUAN_MK2: VWCarInfo("Volkswagen Tiguan 2019-22"), + CAR.TIGUAN_MK2: VWCarInfo("Volkswagen Tiguan 2018-23"), CAR.TOURAN_MK2: VWCarInfo("Volkswagen Touran 2017"), CAR.TRANSPORTER_T61: [ VWCarInfo("Volkswagen Caravelle 2020"), @@ -352,6 +360,23 @@ FW_VERSIONS = { b'\xf1\x875Q0907572P \xf1\x890682', ], }, + CAR.CRAFTER_MK2: { + (Ecu.engine, 0x7e0, None): [ + b'\xf1\x8704L906056EK\xf1\x896391', + ], + # Only current upstreamed vehicle has a manual transmission + #(Ecu.transmission, 0x7e1, None): [ + #], + (Ecu.srs, 0x715, None): [ + b'\xf1\x873Q0959655BG\xf1\x890703\xf1\x82\x0e16120016130012051G1313052900', + ], + (Ecu.eps, 0x712, None): [ + b'\xf1\x872N0909143E \xf1\x897021\xf1\x82\x05163AZ306A2', + ], + (Ecu.fwdRadar, 0x757, None): [ + b'\xf1\x872Q0907572M \xf1\x890233', + ], + }, CAR.GOLF_MK7: { (Ecu.engine, 0x7e0, None): [ b'\xf1\x8704E906016A \xf1\x897697', @@ -568,6 +593,7 @@ FW_VERSIONS = { (Ecu.engine, 0x7e0, None): [ b'\xf1\x8703N906026E \xf1\x892114', b'\xf1\x8704E906023AH\xf1\x893379', + b'\xf1\x8704L906026DP\xf1\x891538', b'\xf1\x8704L906026ET\xf1\x891990', b'\xf1\x8704L906026FP\xf1\x892012', b'\xf1\x8704L906026GA\xf1\x892013', @@ -586,6 +612,7 @@ FW_VERSIONS = { ], (Ecu.srs, 0x715, None): [ b'\xf1\x873Q0959655AE\xf1\x890195\xf1\x82\r56140056130012416612124111', + b'\xf1\x873Q0959655AF\xf1\x890195\xf1\x82\r56140056130012026612120211', b'\xf1\x873Q0959655AN\xf1\x890306\xf1\x82\r58160058140013036914110311', b'\xf1\x873Q0959655BA\xf1\x890195\xf1\x82\r56140056130012516612125111', b'\xf1\x873Q0959655BB\xf1\x890195\xf1\x82\r56140056130012026612120211', @@ -596,6 +623,7 @@ FW_VERSIONS = { (Ecu.eps, 0x712, None): [ b'\xf1\x873Q0909144J \xf1\x895063\xf1\x82\x0566B00611A1', b'\xf1\x873Q0909144J \xf1\x895063\xf1\x82\x0566B00711A1', + b'\xf1\x875Q0909143K \xf1\x892033\xf1\x820514B0060703', b'\xf1\x875Q0909143M \xf1\x892041\xf1\x820522B0060803', b'\xf1\x875Q0909143M \xf1\x892041\xf1\x820522B0080803', b'\xf1\x875Q0909144AB\xf1\x891082\xf1\x82\00521B00606A1', @@ -604,6 +632,7 @@ FW_VERSIONS = { b'\xf1\x875Q0910143C \xf1\x892211\xf1\x82\x0567B0020600', ], (Ecu.fwdRadar, 0x757, None): [ + b'\xf1\x873Q0907572A \xf1\x890126', b'\xf1\x873Q0907572A \xf1\x890130', b'\xf1\x873Q0907572B \xf1\x890192', b'\xf1\x873Q0907572C \xf1\x890195', @@ -700,39 +729,49 @@ FW_VERSIONS = { }, CAR.TIGUAN_MK2: { (Ecu.engine, 0x7e0, None): [ + b'\xf1\x8703N906026D \xf1\x893680', b'\xf1\x8704E906027NB\xf1\x899504', b'\xf1\x8704L906026EJ\xf1\x893661', b'\xf1\x8704L906027G \xf1\x899893', b'\xf1\x875N0906259 \xf1\x890002', + b'\xf1\x875NA906259H \xf1\x890002', b'\xf1\x875NA907115E \xf1\x890005', b'\xf1\x8783A907115B \xf1\x890005', + b'\xf1\x8783A907115F \xf1\x890002', b'\xf1\x8783A907115G \xf1\x890001', ], (Ecu.transmission, 0x7e1, None): [ b'\xf1\x8709G927158DT\xf1\x893698', + b'\xf1\x8709G927158FM\xf1\x893757', b'\xf1\x8709G927158GC\xf1\x893821', b'\xf1\x8709G927158GD\xf1\x893820', b'\xf1\x870D9300043 \xf1\x895202', b'\xf1\x870DL300011N \xf1\x892001', b'\xf1\x870DL300011N \xf1\x892012', + b'\xf1\x870DL300012P \xf1\x892103', b'\xf1\x870DL300013A \xf1\x893005', b'\xf1\x870DL300013G \xf1\x892119', b'\xf1\x870DL300013G \xf1\x892120', + b'\xf1\x870DL300014C \xf1\x893703', ], (Ecu.srs, 0x715, None): [ b'\xf1\x875Q0959655AR\xf1\x890317\xf1\x82\02331310031333334313132573732379333313100', + b'\xf1\x875Q0959655BJ\xf1\x890336\xf1\x82\x1312110031333300314232583732379333423100', b'\xf1\x875Q0959655BM\xf1\x890403\xf1\x82\02316143231313500314641011750179333423100', b'\xf1\x875Q0959655BT\xf1\x890403\xf1\x82\02312110031333300314240583752379333423100', b'\xf1\x875Q0959655BT\xf1\x890403\xf1\x82\02331310031333336313140013950399333423100', b'\xf1\x875Q0959655BT\xf1\x890403\xf1\x82\x1331310031333334313140013750379333423100', b'\xf1\x875Q0959655BT\xf1\x890403\xf1\x82\x1331310031333334313140573752379333423100', b'\xf1\x875Q0959655CB\xf1\x890421\xf1\x82\x1316143231313500314647021750179333613100', + b'\xf1\x875Q0959655CG\xf1\x890421\xf1\x82\x1331310031333300314240024050409333613100', ], (Ecu.eps, 0x712, None): [ b'\xf1\x875Q0909143M \xf1\x892041\xf1\x820529A6060603', b'\xf1\x875Q0909144AB\xf1\x891082\xf1\x82\x0521A60604A1', b'\xf1\x875Q0910143C \xf1\x892211\xf1\x82\x0567A6000600', + b'\xf1\x875QF909144A \xf1\x895581\xf1\x82\x0571A60834A1', b'\xf1\x875QF909144B \xf1\x895582\xf1\x82\00571A60634A1', + b'\xf1\x875QF909144B \xf1\x895582\xf1\x82\x0571A62A32A1', b'\xf1\x875QM909144B \xf1\x891081\xf1\x82\x0521A60604A1', b'\xf1\x875QM909144C \xf1\x891082\xf1\x82\x0521A60604A1', b'\xf1\x875QM909144C \xf1\x891082\xf1\x82\00521A60804A1', @@ -740,6 +779,7 @@ FW_VERSIONS = { (Ecu.fwdRadar, 0x757, None): [ b'\xf1\x872Q0907572AA\xf1\x890396', b'\xf1\x872Q0907572J \xf1\x890156', + b'\xf1\x872Q0907572M \xf1\x890233', b'\xf1\x872Q0907572Q \xf1\x890342', b'\xf1\x872Q0907572R \xf1\x890372', b'\xf1\x872Q0907572T \xf1\x890383', @@ -936,16 +976,19 @@ FW_VERSIONS = { b'\xf1\x8704L906021EL\xf1\x897542', b'\xf1\x8704L906026BP\xf1\x891198', b'\xf1\x8704L906026BP\xf1\x897608', + b'\xf1\x8704L906056CR\xf1\x892797', b'\xf1\x8705E906018AS\xf1\x899596', b'\xf1\x878V0906264H \xf1\x890005', ], (Ecu.transmission, 0x7e1, None): [ + b'\xf1\x870CW300041D \xf1\x891004', b'\xf1\x870CW300041G \xf1\x891003', b'\xf1\x870CW300050J \xf1\x891908', b'\xf1\x870D9300042M \xf1\x895016', ], (Ecu.srs, 0x715, None): [ b'\xf1\x873Q0959655AC\xf1\x890189\xf1\x82\r11110011110011021511110200', + b'\xf1\x873Q0959655AS\xf1\x890200\xf1\x82\r11110011110011021511110200', b'\xf1\x873Q0959655AS\xf1\x890200\xf1\x82\r12110012120012021612110200', b'\xf1\x873Q0959655BH\xf1\x890703\xf1\x82\x0e1312001313001305171311052900', b'\xf1\x873Q0959655CM\xf1\x890720\xf1\x82\0161312001313001305171311052900', @@ -959,6 +1002,7 @@ FW_VERSIONS = { (Ecu.fwdRadar, 0x757, None): [ b'\xf1\x875Q0907572B \xf1\x890200\xf1\x82\00101', b'\xf1\x875Q0907572H \xf1\x890620', + b'\xf1\x875Q0907572K \xf1\x890402\xf1\x82\x0101', b'\xf1\x875Q0907572P \xf1\x890682', ], }, @@ -1079,6 +1123,7 @@ FW_VERSIONS = { b'\xf1\x870CW300050 \xf1\x891709', ], (Ecu.srs, 0x715, None): [ + b'\xf1\x872Q0959655AJ\xf1\x890250\xf1\x82\x1211110411110411--04040404131111112H14', b'\xf1\x872Q0959655AM\xf1\x890351\xf1\x82\022111104111104112104040404111111112H14', ], (Ecu.eps, 0x712, None): [ diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 1d92cd4fb2..2c359ec32a 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -99,7 +99,8 @@ class Controls: get_one_can(self.can_sock) num_pandas = len(messaging.recv_one_retry(self.sm.sock['pandaStates']).pandaStates) - self.CI, self.CP = get_car(self.can_sock, self.pm.sock['sendcan'], num_pandas) + experimental_long_allowed = self.params.get_bool("ExperimentalLongitudinalEnabled") and not is_release_branch() + self.CI, self.CP = get_car(self.can_sock, self.pm.sock['sendcan'], experimental_long_allowed, num_pandas) else: self.CI, self.CP = CI, CI.CP @@ -132,9 +133,6 @@ class Controls: safety_config.safetyModel = car.CarParams.SafetyModel.noOutput self.CP.safetyConfigs = [safety_config] - if is_release_branch(): - self.CP.experimentalLongitudinalAvailable = False - # Write CarParams for radard cp_bytes = self.CP.to_bytes() self.params.put("CarParams", cp_bytes) @@ -142,7 +140,7 @@ class Controls: put_nonblocking("CarParamsPersistent", cp_bytes) # cleanup old params - if not self.CP.experimentalLongitudinalAvailable: + if not self.CP.experimentalLongitudinalAvailable or is_release_branch(): self.params.remove("ExperimentalLongitudinalEnabled") if not self.CP.openpilotLongitudinalControl: self.params.remove("ExperimentalMode") @@ -169,12 +167,13 @@ class Controls: self.state = State.disabled self.enabled = False self.active = False - self.can_rcv_timeout = False self.soft_disable_timer = 0 self.mismatch_counter = 0 self.cruise_mismatch_counter = 0 - self.can_rcv_timeout_counter = 0 + self.can_rcv_timeout_counter = 0 # conseuctive timeout count + self.can_rcv_cum_timeout_counter = 0 # cumulative timeout count self.last_blinker_frame = 0 + self.last_steering_pressed_frame = 0 self.distance_traveled = 0 self.last_functional_fan_frame = 0 self.events_prev = [] @@ -184,10 +183,12 @@ class Controls: self.steer_limited = False self.desired_curvature = 0.0 self.desired_curvature_rate = 0.0 + self.experimental_mode = False self.v_cruise_helper = VCruiseHelper(self.CP) # TODO: no longer necessary, aside from process replay self.sm['liveParameters'].valid = True + self.can_log_mono_time = 0 self.startup_event = get_startup_event(car_recognized, controller_available, len(self.CP.carFw) > 0) @@ -349,9 +350,10 @@ class Controls: self.events.add(EventName.canError) # generic catch-all. ideally, a more specific event should be added above instead + can_rcv_timeout = self.can_rcv_timeout_counter >= 5 has_disable_events = self.events.any(ET.NO_ENTRY) and (self.events.any(ET.SOFT_DISABLE) or self.events.any(ET.IMMEDIATE_DISABLE)) no_system_errors = (not has_disable_events) or (len(self.events) == num_events) - if (not self.sm.all_checks() or self.can_rcv_timeout) and no_system_errors: + if (not self.sm.all_checks() or can_rcv_timeout) and no_system_errors: if not self.sm.all_alive(): self.events.add(EventName.commIssue) elif not self.sm.all_freq_ok(): @@ -363,7 +365,7 @@ class Controls: '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], 'not_freq_ok': [s for s, freq_ok in self.sm.freq_ok.items() if not freq_ok], - 'can_rcv_timeout': self.can_rcv_timeout, + 'can_rcv_timeout': can_rcv_timeout, } if logs != self.logged_comm_issue: cloudlog.event("commIssue", error=True, **logs) @@ -426,6 +428,8 @@ class Controls: # Update carState from CAN can_strs = messaging.drain_sock_raw(self.can_sock, wait_for_one=True) CS = self.CI.update(self.CC, can_strs) + if len(can_strs) and REPLAY: + self.can_log_mono_time = messaging.log_from_bytes(can_strs[0]).logMonoTime self.sm.update(0) @@ -443,9 +447,9 @@ class Controls: # Check for CAN timeout if not can_strs: self.can_rcv_timeout_counter += 1 - self.can_rcv_timeout = True + self.can_rcv_cum_timeout_counter += 1 else: - self.can_rcv_timeout = False + self.can_rcv_timeout_counter = 0 # When the panda and controlsd do not agree on controls_allowed # we want to disengage openpilot. However the status from the panda goes through @@ -541,7 +545,7 @@ class Controls: else: self.state = State.enabled self.current_alert_types.append(ET.ENABLE) - self.v_cruise_helper.initialize_v_cruise(CS) + self.v_cruise_helper.initialize_v_cruise(CS, self.experimental_mode) # Check if openpilot is engaged and actuators are enabled self.enabled = self.state in ENABLED_STATES @@ -608,6 +612,7 @@ class Controls: actuators.steer, actuators.steeringAngleDeg, lac_log = self.LaC.update(CC.latActive, CS, self.VM, lp, self.last_actuators, self.steer_limited, self.desired_curvature, self.desired_curvature_rate, self.sm['liveLocationKalman']) + actuators.curvature = self.desired_curvature else: lac_log = log.ControlsState.LateralDebugState.new_message() if self.sm.rcv_frame['testJoystick'] > 0: @@ -624,29 +629,34 @@ class Controls: lac_log.output = actuators.steer lac_log.saturated = abs(actuators.steer) >= 0.9 + if CS.steeringPressed: + self.last_steering_pressed_frame = self.sm.frame + recent_steer_pressed = (self.sm.frame - self.last_steering_pressed_frame)*DT_CTRL < 2.0 + # Send a "steering required alert" if saturation count has reached the limit - if lac_log.active and not CS.steeringPressed and self.CP.lateralTuning.which() == 'torque' and not self.joystick_mode: - undershooting = abs(lac_log.desiredLateralAccel) / abs(1e-3 + lac_log.actualLateralAccel) > 1.2 - turning = abs(lac_log.desiredLateralAccel) > 1.0 - good_speed = CS.vEgo > 5 - max_torque = abs(self.last_actuators.steer) > 0.99 - if undershooting and turning and good_speed and max_torque: - self.events.add(EventName.steerSaturated) - elif lac_log.active and lac_log.saturated: - dpath_points = lat_plan.dPathPoints - if len(dpath_points): - # Check if we deviated from the path - # TODO use desired vs actual curvature - if self.CP.steerControlType == car.CarParams.SteerControlType.angle: - steering_value = actuators.steeringAngleDeg - else: - steering_value = actuators.steer + if lac_log.active and not recent_steer_pressed: + if self.CP.lateralTuning.which() == 'torque' and not self.joystick_mode: + undershooting = abs(lac_log.desiredLateralAccel) / abs(1e-3 + lac_log.actualLateralAccel) > 1.2 + turning = abs(lac_log.desiredLateralAccel) > 1.0 + good_speed = CS.vEgo > 5 + max_torque = abs(self.last_actuators.steer) > 0.99 + if undershooting and turning and good_speed and max_torque: + lac_log.active and self.events.add(EventName.steerSaturated) + elif lac_log.saturated: + dpath_points = lat_plan.dPathPoints + if len(dpath_points): + # Check if we deviated from the path + # TODO use desired vs actual curvature + if self.CP.steerControlType == car.CarParams.SteerControlType.angle: + steering_value = actuators.steeringAngleDeg + else: + steering_value = actuators.steer - left_deviation = steering_value > 0 and dpath_points[0] < -0.20 - right_deviation = steering_value < 0 and dpath_points[0] > 0.20 + left_deviation = steering_value > 0 and dpath_points[0] < -0.20 + right_deviation = steering_value < 0 and dpath_points[0] > 0.20 - if left_deviation or right_deviation: - self.events.add(EventName.steerSaturated) + if left_deviation or right_deviation: + self.events.add(EventName.steerSaturated) # Ensure no NaNs/Infs for p in ACTUATOR_FIELDS: @@ -726,7 +736,8 @@ class Controls: if not self.read_only and self.initialized: # send car controls over can - self.last_actuators, can_sends = self.CI.apply(CC) + now_nanos = self.can_log_mono_time if REPLAY else int(sec_since_boot() * 1e9) + self.last_actuators, can_sends = self.CI.apply(CC, now_nanos) self.pm.send('sendcan', can_list_to_can_capnp(can_sends, msgtype='sendcan', valid=CS.canValid)) CC.actuatorsOutput = self.last_actuators if self.CP.steerControlType == car.CarParams.SteerControlType.angle: @@ -757,7 +768,6 @@ class Controls: controlsState.alertType = current_alert.alert_type controlsState.alertSound = current_alert.audible_alert - controlsState.canMonoTimes = list(CS.canMonoTimes) controlsState.longitudinalPlanMonoTime = self.sm.logMonoTime['longitudinalPlan'] controlsState.lateralPlanMonoTime = self.sm.logMonoTime['lateralPlan'] controlsState.enabled = self.enabled @@ -777,8 +787,8 @@ class Controls: controlsState.cumLagMs = -self.rk.remaining * 1000. controlsState.startMonoTime = int(start_time * 1e9) controlsState.forceDecel = bool(force_decel) - controlsState.canErrorCounter = self.can_rcv_timeout_counter - controlsState.experimentalMode = self.params.get_bool("ExperimentalMode") and self.CP.openpilotLongitudinalControl + controlsState.canErrorCounter = self.can_rcv_cum_timeout_counter + controlsState.experimentalMode = self.experimental_mode lat_tuning = self.CP.lateralTuning.which() if self.joystick_mode: @@ -829,6 +839,7 @@ class Controls: self.prof.checkpoint("Ratekeeper", ignore=True) self.is_metric = self.params.get_bool("IsMetric") + self.experimental_mode = self.params.get_bool("ExperimentalMode") and self.CP.openpilotLongitudinalControl # Sample data from sockets and get a carState CS = self.data_sample() diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index 3d5ec8ac1d..a332d06765 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/selfdrive/controls/lib/drive_helpers.py @@ -8,10 +8,12 @@ from selfdrive.modeld.constants import T_IDXS # WARNING: this value was determined based on the model's training distribution, # model predictions above this speed can be unpredictable -V_CRUISE_MAX = 145 # kph -V_CRUISE_MIN = 8 # kph -V_CRUISE_ENABLE_MIN = 40 # kph -V_CRUISE_INITIAL = 255 # kph +# V_CRUISE's are in kph +V_CRUISE_MIN = 8 +V_CRUISE_MAX = 145 +V_CRUISE_UNSET = 255 +V_CRUISE_INITIAL = 40 +V_CRUISE_INITIAL_EXPERIMENTAL_MODE = 105 IMPERIAL_INCREMENT = 1.6 # should be CV.MPH_TO_KPH, but this causes rounding errors MIN_SPEED = 1.0 @@ -39,15 +41,15 @@ CRUISE_INTERVAL_SIGN = { class VCruiseHelper: def __init__(self, CP): self.CP = CP - self.v_cruise_kph = V_CRUISE_INITIAL - self.v_cruise_cluster_kph = V_CRUISE_INITIAL + self.v_cruise_kph = V_CRUISE_UNSET + self.v_cruise_cluster_kph = V_CRUISE_UNSET self.v_cruise_kph_last = 0 self.button_timers = {ButtonType.decelCruise: 0, ButtonType.accelCruise: 0} self.button_change_states = {btn: {"standstill": False, "enabled": False} for btn in self.button_timers} @property def v_cruise_initialized(self): - return self.v_cruise_kph != V_CRUISE_INITIAL + return self.v_cruise_kph != V_CRUISE_UNSET def update_v_cruise(self, CS, enabled, is_metric): self.v_cruise_kph_last = self.v_cruise_kph @@ -62,8 +64,8 @@ class VCruiseHelper: self.v_cruise_kph = CS.cruiseState.speed * CV.MS_TO_KPH self.v_cruise_cluster_kph = CS.cruiseState.speedCluster * CV.MS_TO_KPH else: - self.v_cruise_kph = V_CRUISE_INITIAL - self.v_cruise_cluster_kph = V_CRUISE_INITIAL + self.v_cruise_kph = V_CRUISE_UNSET + self.v_cruise_cluster_kph = V_CRUISE_UNSET def _update_v_cruise_non_pcm(self, CS, enabled, is_metric): # handle button presses. TODO: this should be in state_control, but a decelCruise press @@ -125,16 +127,18 @@ class VCruiseHelper: self.button_timers[b.type.raw] = 1 if b.pressed else 0 self.button_change_states[b.type.raw] = {"standstill": CS.cruiseState.standstill, "enabled": enabled} - def initialize_v_cruise(self, CS): + def initialize_v_cruise(self, CS, experimental_mode: bool) -> None: # initializing is handled by the PCM if self.CP.pcmCruise: return + initial = V_CRUISE_INITIAL_EXPERIMENTAL_MODE if experimental_mode else V_CRUISE_INITIAL + # 250kph or above probably means we never had a set speed if any(b.type in (ButtonType.accelCruise, ButtonType.resumeCruise) for b in CS.buttonEvents) and self.v_cruise_kph_last < 250: self.v_cruise_kph = self.v_cruise_kph_last else: - self.v_cruise_kph = int(round(clip(CS.vEgo * CV.MS_TO_KPH, V_CRUISE_ENABLE_MIN, V_CRUISE_MAX))) + self.v_cruise_kph = int(round(clip(CS.vEgo * CV.MS_TO_KPH, initial, V_CRUISE_MAX))) self.v_cruise_cluster_kph = self.v_cruise_kph diff --git a/selfdrive/controls/lib/events.py b/selfdrive/controls/lib/events.py index 3036828662..6d5ed45076 100644 --- a/selfdrive/controls/lib/events.py +++ b/selfdrive/controls/lib/events.py @@ -729,8 +729,8 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = { }, EventName.espDisabled: { - ET.SOFT_DISABLE: soft_disable_alert("ESP Off"), - ET.NO_ENTRY: NoEntryAlert("ESP Off"), + ET.SOFT_DISABLE: soft_disable_alert("Electronic Stability Control Disabled"), + ET.NO_ENTRY: NoEntryAlert("Electronic Stability Control Disabled"), }, EventName.lowBattery: { diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index db87604e98..34f0f274fe 100755 --- a/selfdrive/controls/radard.py +++ b/selfdrive/controls/radard.py @@ -35,7 +35,7 @@ class KalmanParams(): self.K = [[interp(dt, dts, K0)], [interp(dt, dts, K1)]] -def laplacian_cdf(x, mu, b): +def laplacian_pdf(x, mu, b): b = max(b, 1e-4) return math.exp(-abs(x-mu)/b) @@ -45,9 +45,9 @@ def match_vision_to_cluster(v_ego, lead, clusters): offset_vision_dist = lead.x[0] - RADAR_TO_CAMERA def prob(c): - prob_d = laplacian_cdf(c.dRel, offset_vision_dist, lead.xStd[0]) - prob_y = laplacian_cdf(c.yRel, -lead.y[0], lead.yStd[0]) - prob_v = laplacian_cdf(c.vRel + v_ego, lead.v[0], lead.vStd[0]) + prob_d = laplacian_pdf(c.dRel, offset_vision_dist, lead.xStd[0]) + prob_y = laplacian_pdf(c.yRel, -lead.y[0], lead.yStd[0]) + prob_v = laplacian_pdf(c.vRel + v_ego, lead.v[0], lead.vStd[0]) # This is isn't exactly right, but good heuristic return prob_d * prob_y * prob_v @@ -165,7 +165,6 @@ class RadarD(): dat.valid = sm.all_checks() and len(rr.errors) == 0 radarState = dat.radarState radarState.mdMonoTime = sm.logMonoTime['modelV2'] - radarState.canMonoTimes = list(rr.canMonoTimes) radarState.radarErrors = list(rr.errors) radarState.carStateMonoTime = sm.logMonoTime['carState'] diff --git a/selfdrive/controls/tests/test_cruise_speed.py b/selfdrive/controls/tests/test_cruise_speed.py index cd1d31cf07..b83116af45 100755 --- a/selfdrive/controls/tests/test_cruise_speed.py +++ b/selfdrive/controls/tests/test_cruise_speed.py @@ -3,7 +3,7 @@ import numpy as np from parameterized import parameterized_class import unittest -from selfdrive.controls.lib.drive_helpers import VCruiseHelper, V_CRUISE_MIN, V_CRUISE_MAX, V_CRUISE_ENABLE_MIN, IMPERIAL_INCREMENT +from selfdrive.controls.lib.drive_helpers import VCruiseHelper, V_CRUISE_MIN, V_CRUISE_MAX, V_CRUISE_INITIAL, IMPERIAL_INCREMENT from cereal import car from common.conversions import Conversions as CV from selfdrive.test.longitudinal_maneuvers.maneuver import Maneuver @@ -53,16 +53,16 @@ class TestVCruiseHelper(unittest.TestCase): for _ in range(2): self.v_cruise_helper.update_v_cruise(car.CarState(cruiseState={"available": False}), enabled=False, is_metric=False) - def enable(self, v_ego): + def enable(self, v_ego, experimental_mode): # Simulates user pressing set with a current speed - self.v_cruise_helper.initialize_v_cruise(car.CarState(vEgo=v_ego)) + self.v_cruise_helper.initialize_v_cruise(car.CarState(vEgo=v_ego), experimental_mode) def test_adjust_speed(self): """ Asserts speed changes on falling edges of buttons. """ - self.enable(V_CRUISE_ENABLE_MIN * CV.KPH_TO_MS) + self.enable(V_CRUISE_INITIAL * CV.KPH_TO_MS, False) for btn in (ButtonType.accelCruise, ButtonType.decelCruise): for pressed in (True, False): @@ -86,7 +86,7 @@ class TestVCruiseHelper(unittest.TestCase): CS.buttonEvents = [ButtonEvent(type=ButtonType.decelCruise, pressed=pressed)] self.v_cruise_helper.update_v_cruise(CS, enabled=enabled, is_metric=False) if pressed: - self.enable(V_CRUISE_ENABLE_MIN * CV.KPH_TO_MS) + self.enable(V_CRUISE_INITIAL * CV.KPH_TO_MS, False) # Expected diff on enabling. Speed should not change on falling edge of pressed self.assertEqual(not pressed, self.v_cruise_helper.v_cruise_kph == self.v_cruise_helper.v_cruise_kph_last) @@ -96,7 +96,7 @@ class TestVCruiseHelper(unittest.TestCase): Asserts we don't increment set speed if user presses resume/accel to exit cruise standstill. """ - self.enable(0) + self.enable(0, False) for standstill in (True, False): for pressed in (True, False): @@ -116,7 +116,7 @@ class TestVCruiseHelper(unittest.TestCase): for v_ego in np.linspace(0, 100, 101): self.reset_cruise_speed_state() - self.enable(V_CRUISE_ENABLE_MIN * CV.KPH_TO_MS) + self.enable(V_CRUISE_INITIAL * CV.KPH_TO_MS, False) # first decrement speed, then perform gas pressed logic expected_v_cruise_kph = self.v_cruise_helper.v_cruise_kph - IMPERIAL_INCREMENT @@ -137,13 +137,14 @@ class TestVCruiseHelper(unittest.TestCase): Asserts allowed cruise speeds on enabling with SET. """ - for v_ego in np.linspace(0, 100, 101): - self.reset_cruise_speed_state() - self.assertFalse(self.v_cruise_helper.v_cruise_initialized) + for experimental_mode in (True, False): + for v_ego in np.linspace(0, 100, 101): + self.reset_cruise_speed_state() + self.assertFalse(self.v_cruise_helper.v_cruise_initialized) - self.enable(float(v_ego)) - self.assertTrue(V_CRUISE_ENABLE_MIN <= self.v_cruise_helper.v_cruise_kph <= V_CRUISE_MAX) - self.assertTrue(self.v_cruise_helper.v_cruise_initialized) + self.enable(float(v_ego), experimental_mode) + self.assertTrue(V_CRUISE_INITIAL <= self.v_cruise_helper.v_cruise_kph <= V_CRUISE_MAX) + self.assertTrue(self.v_cruise_helper.v_cruise_initialized) if __name__ == "__main__": diff --git a/selfdrive/debug/hyundai_enable_radar_points.py b/selfdrive/debug/hyundai_enable_radar_points.py index ac7e7102d0..07ce5ebddb 100755 --- a/selfdrive/debug/hyundai_enable_radar_points.py +++ b/selfdrive/debug/hyundai_enable_radar_points.py @@ -52,6 +52,10 @@ SUPPORTED_FW_VERSIONS = { b'IK__ SCC F-CUP 1.00 1.02 96400-G9100\x18\x07\x06\x17\x12 ': ConfigValues( default_config=b"\x00\x00\x00\x01\x00\x00", tracks_enabled=b"\x00\x00\x00\x01\x00\x01"), + # 2019 SANTA FE + b"TM__ SCC F-CUP 1.00 1.00 99110-S1210\x19\x01%\x168 ": ConfigValues( + default_config=b"\x00\x00\x00\x01\x00\x00", + tracks_enabled=b"\x00\x00\x00\x01\x00\x01"), } if __name__ == "__main__": diff --git a/selfdrive/debug/internal/fuzz_fw_fingerprint.py b/selfdrive/debug/internal/fuzz_fw_fingerprint.py index 1ea133cc19..a18390fef3 100755 --- a/selfdrive/debug/internal/fuzz_fw_fingerprint.py +++ b/selfdrive/debug/internal/fuzz_fw_fingerprint.py @@ -28,7 +28,7 @@ if __name__ == "__main__": for candidate, fws in FWS.items(): fw_dict = {} for (tp, addr, subaddr), fw_list in fws.items(): - fw_dict[(addr, subaddr)] = random.choice(fw_list) + fw_dict[(addr, subaddr)] = [random.choice(fw_list)] matches = match_fw_to_car_fuzzy(fw_dict, log=False, exclude=candidate) diff --git a/selfdrive/debug/vw_mqb_config.py b/selfdrive/debug/vw_mqb_config.py index 8c4dbc55ee..8952405b8e 100755 --- a/selfdrive/debug/vw_mqb_config.py +++ b/selfdrive/debug/vw_mqb_config.py @@ -126,6 +126,7 @@ if __name__ == "__main__": uds_client.security_access(ACCESS_TYPE_LEVEL_1.SEND_KEY, struct.pack("!I", key)) # type: ignore except (NegativeResponseError, MessageTimeoutError): print("Security access failed!") + print("Open the hood and retry (disables the \"diagnostic firewall\" on newer vehicles)") quit() try: diff --git a/selfdrive/locationd/.gitignore b/selfdrive/locationd/.gitignore index 5268902785..86a228a6ff 100644 --- a/selfdrive/locationd/.gitignore +++ b/selfdrive/locationd/.gitignore @@ -3,3 +3,4 @@ ubloxd_test params_learner paramsd locationd +test/test_glonass_runner diff --git a/selfdrive/locationd/SConscript b/selfdrive/locationd/SConscript index 4b7fba19b6..61a0ed7f42 100644 --- a/selfdrive/locationd/SConscript +++ b/selfdrive/locationd/SConscript @@ -7,8 +7,14 @@ if GetOption('kaitai'): cmd = f"kaitai-struct-compiler --target cpp_stl --outdir {generated} $SOURCES" env.Command(['generated/ubx.cpp', 'generated/ubx.h'], 'ubx.ksy', cmd) env.Command(['generated/gps.cpp', 'generated/gps.h'], 'gps.ksy', cmd) + glonass = env.Command(['generated/glonass.cpp', 'generated/glonass.h'], 'glonass.ksy', cmd) -env.Program("ubloxd", ["ubloxd.cc", "ublox_msg.cc", "generated/ubx.cpp", "generated/gps.cpp"], LIBS=loc_libs) + # kaitai issue: https://github.com/kaitai-io/kaitai_struct/issues/910 + patch = env.Command(None, 'glonass_fix.patch', 'git apply $SOURCES') + env.Depends(patch, glonass) + +glonass_obj = env.Object('generated/glonass.cpp') +env.Program("ubloxd", ["ubloxd.cc", "ublox_msg.cc", "generated/ubx.cpp", "generated/gps.cpp", glonass_obj], LIBS=loc_libs) ekf_sym_cc = env.SharedObject("#rednose/helpers/ekf_sym.cc") locationd_sources = ["locationd.cc", "models/live_kf.cc", ekf_sym_cc] @@ -20,3 +26,6 @@ lenv.Depends(locationd, libkf) if File("liblocationd.cc").exists(): liblocationd = lenv.SharedLibrary("liblocationd", ["liblocationd.cc"] + locationd_sources, LIBS=loc_libs + transformations) lenv.Depends(liblocationd, libkf) + +if GetOption('test'): + env.Program("test/test_glonass_runner", ['test/test_glonass_runner.cc', 'test/test_glonass_kaitai.cc', glonass_obj], LIBS=[loc_libs]) \ No newline at end of file diff --git a/selfdrive/locationd/generated/glonass.cpp b/selfdrive/locationd/generated/glonass.cpp new file mode 100644 index 0000000000..cd0f96ab68 --- /dev/null +++ b/selfdrive/locationd/generated/glonass.cpp @@ -0,0 +1,353 @@ +// This is a generated file! Please edit source .ksy file and use kaitai-struct-compiler to rebuild + +#include "glonass.h" + +glonass_t::glonass_t(kaitai::kstream* p__io, kaitai::kstruct* p__parent, glonass_t* p__root) : kaitai::kstruct(p__io) { + m__parent = p__parent; + m__root = this; + + try { + _read(); + } catch(...) { + _clean_up(); + throw; + } +} + +void glonass_t::_read() { + m_idle_chip = m__io->read_bits_int_be(1); + m_string_number = m__io->read_bits_int_be(4); + //m__io->align_to_byte(); + switch (string_number()) { + case 4: { + m_data = new string_4_t(m__io, this, m__root); + break; + } + case 1: { + m_data = new string_1_t(m__io, this, m__root); + break; + } + case 3: { + m_data = new string_3_t(m__io, this, m__root); + break; + } + case 5: { + m_data = new string_5_t(m__io, this, m__root); + break; + } + case 2: { + m_data = new string_2_t(m__io, this, m__root); + break; + } + default: { + m_data = new string_non_immediate_t(m__io, this, m__root); + break; + } + } + m_hamming_code = m__io->read_bits_int_be(8); + m_pad_1 = m__io->read_bits_int_be(11); + m_superframe_number = m__io->read_bits_int_be(16); + m_pad_2 = m__io->read_bits_int_be(8); + m_frame_number = m__io->read_bits_int_be(8); +} + +glonass_t::~glonass_t() { + _clean_up(); +} + +void glonass_t::_clean_up() { + if (m_data) { + delete m_data; m_data = 0; + } +} + +glonass_t::string_4_t::string_4_t(kaitai::kstream* p__io, glonass_t* p__parent, glonass_t* p__root) : kaitai::kstruct(p__io) { + m__parent = p__parent; + m__root = p__root; + f_tau_n = false; + f_delta_tau_n = false; + + try { + _read(); + } catch(...) { + _clean_up(); + throw; + } +} + +void glonass_t::string_4_t::_read() { + m_tau_n_sign = m__io->read_bits_int_be(1); + m_tau_n_value = m__io->read_bits_int_be(21); + m_delta_tau_n_sign = m__io->read_bits_int_be(1); + m_delta_tau_n_value = m__io->read_bits_int_be(4); + m_e_n = m__io->read_bits_int_be(5); + m_not_used_1 = m__io->read_bits_int_be(14); + m_p4 = m__io->read_bits_int_be(1); + m_f_t = m__io->read_bits_int_be(4); + m_not_used_2 = m__io->read_bits_int_be(3); + m_n_t = m__io->read_bits_int_be(11); + m_n = m__io->read_bits_int_be(5); + m_m = m__io->read_bits_int_be(2); +} + +glonass_t::string_4_t::~string_4_t() { + _clean_up(); +} + +void glonass_t::string_4_t::_clean_up() { +} + +int32_t glonass_t::string_4_t::tau_n() { + if (f_tau_n) + return m_tau_n; + m_tau_n = ((tau_n_sign()) ? ((tau_n_value() * -1)) : (tau_n_value())); + f_tau_n = true; + return m_tau_n; +} + +int32_t glonass_t::string_4_t::delta_tau_n() { + if (f_delta_tau_n) + return m_delta_tau_n; + m_delta_tau_n = ((delta_tau_n_sign()) ? ((delta_tau_n_value() * -1)) : (delta_tau_n_value())); + f_delta_tau_n = true; + return m_delta_tau_n; +} + +glonass_t::string_non_immediate_t::string_non_immediate_t(kaitai::kstream* p__io, glonass_t* p__parent, glonass_t* p__root) : kaitai::kstruct(p__io) { + m__parent = p__parent; + m__root = p__root; + + try { + _read(); + } catch(...) { + _clean_up(); + throw; + } +} + +void glonass_t::string_non_immediate_t::_read() { + m_data_1 = m__io->read_bits_int_be(64); + m_data_2 = m__io->read_bits_int_be(8); +} + +glonass_t::string_non_immediate_t::~string_non_immediate_t() { + _clean_up(); +} + +void glonass_t::string_non_immediate_t::_clean_up() { +} + +glonass_t::string_5_t::string_5_t(kaitai::kstream* p__io, glonass_t* p__parent, glonass_t* p__root) : kaitai::kstruct(p__io) { + m__parent = p__parent; + m__root = p__root; + + try { + _read(); + } catch(...) { + _clean_up(); + throw; + } +} + +void glonass_t::string_5_t::_read() { + m_n_a = m__io->read_bits_int_be(11); + m_tau_c = m__io->read_bits_int_be(32); + m_not_used = m__io->read_bits_int_be(1); + m_n_4 = m__io->read_bits_int_be(5); + m_tau_gps = m__io->read_bits_int_be(22); + m_l_n = m__io->read_bits_int_be(1); +} + +glonass_t::string_5_t::~string_5_t() { + _clean_up(); +} + +void glonass_t::string_5_t::_clean_up() { +} + +glonass_t::string_1_t::string_1_t(kaitai::kstream* p__io, glonass_t* p__parent, glonass_t* p__root) : kaitai::kstruct(p__io) { + m__parent = p__parent; + m__root = p__root; + f_x_vel = false; + f_x_accel = false; + f_x = false; + + try { + _read(); + } catch(...) { + _clean_up(); + throw; + } +} + +void glonass_t::string_1_t::_read() { + m_not_used = m__io->read_bits_int_be(2); + m_p1 = m__io->read_bits_int_be(2); + m_t_k = m__io->read_bits_int_be(12); + m_x_vel_sign = m__io->read_bits_int_be(1); + m_x_vel_value = m__io->read_bits_int_be(23); + m_x_accel_sign = m__io->read_bits_int_be(1); + m_x_accel_value = m__io->read_bits_int_be(4); + m_x_sign = m__io->read_bits_int_be(1); + m_x_value = m__io->read_bits_int_be(26); +} + +glonass_t::string_1_t::~string_1_t() { + _clean_up(); +} + +void glonass_t::string_1_t::_clean_up() { +} + +int32_t glonass_t::string_1_t::x_vel() { + if (f_x_vel) + return m_x_vel; + m_x_vel = ((x_vel_sign()) ? ((x_vel_value() * -1)) : (x_vel_value())); + f_x_vel = true; + return m_x_vel; +} + +int32_t glonass_t::string_1_t::x_accel() { + if (f_x_accel) + return m_x_accel; + m_x_accel = ((x_accel_sign()) ? ((x_accel_value() * -1)) : (x_accel_value())); + f_x_accel = true; + return m_x_accel; +} + +int32_t glonass_t::string_1_t::x() { + if (f_x) + return m_x; + m_x = ((x_sign()) ? ((x_value() * -1)) : (x_value())); + f_x = true; + return m_x; +} + +glonass_t::string_2_t::string_2_t(kaitai::kstream* p__io, glonass_t* p__parent, glonass_t* p__root) : kaitai::kstruct(p__io) { + m__parent = p__parent; + m__root = p__root; + f_y_vel = false; + f_y_accel = false; + f_y = false; + + try { + _read(); + } catch(...) { + _clean_up(); + throw; + } +} + +void glonass_t::string_2_t::_read() { + m_b_n = m__io->read_bits_int_be(3); + m_p2 = m__io->read_bits_int_be(1); + m_t_b = m__io->read_bits_int_be(7); + m_not_used = m__io->read_bits_int_be(5); + m_y_vel_sign = m__io->read_bits_int_be(1); + m_y_vel_value = m__io->read_bits_int_be(23); + m_y_accel_sign = m__io->read_bits_int_be(1); + m_y_accel_value = m__io->read_bits_int_be(4); + m_y_sign = m__io->read_bits_int_be(1); + m_y_value = m__io->read_bits_int_be(26); +} + +glonass_t::string_2_t::~string_2_t() { + _clean_up(); +} + +void glonass_t::string_2_t::_clean_up() { +} + +int32_t glonass_t::string_2_t::y_vel() { + if (f_y_vel) + return m_y_vel; + m_y_vel = ((y_vel_sign()) ? ((y_vel_value() * -1)) : (y_vel_value())); + f_y_vel = true; + return m_y_vel; +} + +int32_t glonass_t::string_2_t::y_accel() { + if (f_y_accel) + return m_y_accel; + m_y_accel = ((y_accel_sign()) ? ((y_accel_value() * -1)) : (y_accel_value())); + f_y_accel = true; + return m_y_accel; +} + +int32_t glonass_t::string_2_t::y() { + if (f_y) + return m_y; + m_y = ((y_sign()) ? ((y_value() * -1)) : (y_value())); + f_y = true; + return m_y; +} + +glonass_t::string_3_t::string_3_t(kaitai::kstream* p__io, glonass_t* p__parent, glonass_t* p__root) : kaitai::kstruct(p__io) { + m__parent = p__parent; + m__root = p__root; + f_gamma_n = false; + f_z_vel = false; + f_z_accel = false; + f_z = false; + + try { + _read(); + } catch(...) { + _clean_up(); + throw; + } +} + +void glonass_t::string_3_t::_read() { + m_p3 = m__io->read_bits_int_be(1); + m_gamma_n_sign = m__io->read_bits_int_be(1); + m_gamma_n_value = m__io->read_bits_int_be(10); + m_not_used = m__io->read_bits_int_be(1); + m_p = m__io->read_bits_int_be(2); + m_l_n = m__io->read_bits_int_be(1); + m_z_vel_sign = m__io->read_bits_int_be(1); + m_z_vel_value = m__io->read_bits_int_be(23); + m_z_accel_sign = m__io->read_bits_int_be(1); + m_z_accel_value = m__io->read_bits_int_be(4); + m_z_sign = m__io->read_bits_int_be(1); + m_z_value = m__io->read_bits_int_be(26); +} + +glonass_t::string_3_t::~string_3_t() { + _clean_up(); +} + +void glonass_t::string_3_t::_clean_up() { +} + +int32_t glonass_t::string_3_t::gamma_n() { + if (f_gamma_n) + return m_gamma_n; + m_gamma_n = ((gamma_n_sign()) ? ((gamma_n_value() * -1)) : (gamma_n_value())); + f_gamma_n = true; + return m_gamma_n; +} + +int32_t glonass_t::string_3_t::z_vel() { + if (f_z_vel) + return m_z_vel; + m_z_vel = ((z_vel_sign()) ? ((z_vel_value() * -1)) : (z_vel_value())); + f_z_vel = true; + return m_z_vel; +} + +int32_t glonass_t::string_3_t::z_accel() { + if (f_z_accel) + return m_z_accel; + m_z_accel = ((z_accel_sign()) ? ((z_accel_value() * -1)) : (z_accel_value())); + f_z_accel = true; + return m_z_accel; +} + +int32_t glonass_t::string_3_t::z() { + if (f_z) + return m_z; + m_z = ((z_sign()) ? ((z_value() * -1)) : (z_value())); + f_z = true; + return m_z; +} diff --git a/selfdrive/locationd/generated/glonass.h b/selfdrive/locationd/generated/glonass.h new file mode 100644 index 0000000000..19867ba22b --- /dev/null +++ b/selfdrive/locationd/generated/glonass.h @@ -0,0 +1,375 @@ +#ifndef GLONASS_H_ +#define GLONASS_H_ + +// This is a generated file! Please edit source .ksy file and use kaitai-struct-compiler to rebuild + +#include "kaitai/kaitaistruct.h" +#include + +#if KAITAI_STRUCT_VERSION < 9000L +#error "Incompatible Kaitai Struct C++/STL API: version 0.9 or later is required" +#endif + +class glonass_t : public kaitai::kstruct { + +public: + class string_4_t; + class string_non_immediate_t; + class string_5_t; + class string_1_t; + class string_2_t; + class string_3_t; + + glonass_t(kaitai::kstream* p__io, kaitai::kstruct* p__parent = 0, glonass_t* p__root = 0); + +private: + void _read(); + void _clean_up(); + +public: + ~glonass_t(); + + class string_4_t : public kaitai::kstruct { + + public: + + string_4_t(kaitai::kstream* p__io, glonass_t* p__parent = 0, glonass_t* p__root = 0); + + private: + void _read(); + void _clean_up(); + + public: + ~string_4_t(); + + private: + bool f_tau_n; + int32_t m_tau_n; + + public: + int32_t tau_n(); + + private: + bool f_delta_tau_n; + int32_t m_delta_tau_n; + + public: + int32_t delta_tau_n(); + + private: + bool m_tau_n_sign; + uint64_t m_tau_n_value; + bool m_delta_tau_n_sign; + uint64_t m_delta_tau_n_value; + uint64_t m_e_n; + uint64_t m_not_used_1; + bool m_p4; + uint64_t m_f_t; + uint64_t m_not_used_2; + uint64_t m_n_t; + uint64_t m_n; + uint64_t m_m; + glonass_t* m__root; + glonass_t* m__parent; + + public: + bool tau_n_sign() const { return m_tau_n_sign; } + uint64_t tau_n_value() const { return m_tau_n_value; } + bool delta_tau_n_sign() const { return m_delta_tau_n_sign; } + uint64_t delta_tau_n_value() const { return m_delta_tau_n_value; } + uint64_t e_n() const { return m_e_n; } + uint64_t not_used_1() const { return m_not_used_1; } + bool p4() const { return m_p4; } + uint64_t f_t() const { return m_f_t; } + uint64_t not_used_2() const { return m_not_used_2; } + uint64_t n_t() const { return m_n_t; } + uint64_t n() const { return m_n; } + uint64_t m() const { return m_m; } + glonass_t* _root() const { return m__root; } + glonass_t* _parent() const { return m__parent; } + }; + + class string_non_immediate_t : public kaitai::kstruct { + + public: + + string_non_immediate_t(kaitai::kstream* p__io, glonass_t* p__parent = 0, glonass_t* p__root = 0); + + private: + void _read(); + void _clean_up(); + + public: + ~string_non_immediate_t(); + + private: + uint64_t m_data_1; + uint64_t m_data_2; + glonass_t* m__root; + glonass_t* m__parent; + + public: + uint64_t data_1() const { return m_data_1; } + uint64_t data_2() const { return m_data_2; } + glonass_t* _root() const { return m__root; } + glonass_t* _parent() const { return m__parent; } + }; + + class string_5_t : public kaitai::kstruct { + + public: + + string_5_t(kaitai::kstream* p__io, glonass_t* p__parent = 0, glonass_t* p__root = 0); + + private: + void _read(); + void _clean_up(); + + public: + ~string_5_t(); + + private: + uint64_t m_n_a; + uint64_t m_tau_c; + bool m_not_used; + uint64_t m_n_4; + uint64_t m_tau_gps; + bool m_l_n; + glonass_t* m__root; + glonass_t* m__parent; + + public: + uint64_t n_a() const { return m_n_a; } + uint64_t tau_c() const { return m_tau_c; } + bool not_used() const { return m_not_used; } + uint64_t n_4() const { return m_n_4; } + uint64_t tau_gps() const { return m_tau_gps; } + bool l_n() const { return m_l_n; } + glonass_t* _root() const { return m__root; } + glonass_t* _parent() const { return m__parent; } + }; + + class string_1_t : public kaitai::kstruct { + + public: + + string_1_t(kaitai::kstream* p__io, glonass_t* p__parent = 0, glonass_t* p__root = 0); + + private: + void _read(); + void _clean_up(); + + public: + ~string_1_t(); + + private: + bool f_x_vel; + int32_t m_x_vel; + + public: + int32_t x_vel(); + + private: + bool f_x_accel; + int32_t m_x_accel; + + public: + int32_t x_accel(); + + private: + bool f_x; + int32_t m_x; + + public: + int32_t x(); + + private: + uint64_t m_not_used; + uint64_t m_p1; + uint64_t m_t_k; + bool m_x_vel_sign; + uint64_t m_x_vel_value; + bool m_x_accel_sign; + uint64_t m_x_accel_value; + bool m_x_sign; + uint64_t m_x_value; + glonass_t* m__root; + glonass_t* m__parent; + + public: + uint64_t not_used() const { return m_not_used; } + uint64_t p1() const { return m_p1; } + uint64_t t_k() const { return m_t_k; } + bool x_vel_sign() const { return m_x_vel_sign; } + uint64_t x_vel_value() const { return m_x_vel_value; } + bool x_accel_sign() const { return m_x_accel_sign; } + uint64_t x_accel_value() const { return m_x_accel_value; } + bool x_sign() const { return m_x_sign; } + uint64_t x_value() const { return m_x_value; } + glonass_t* _root() const { return m__root; } + glonass_t* _parent() const { return m__parent; } + }; + + class string_2_t : public kaitai::kstruct { + + public: + + string_2_t(kaitai::kstream* p__io, glonass_t* p__parent = 0, glonass_t* p__root = 0); + + private: + void _read(); + void _clean_up(); + + public: + ~string_2_t(); + + private: + bool f_y_vel; + int32_t m_y_vel; + + public: + int32_t y_vel(); + + private: + bool f_y_accel; + int32_t m_y_accel; + + public: + int32_t y_accel(); + + private: + bool f_y; + int32_t m_y; + + public: + int32_t y(); + + private: + uint64_t m_b_n; + bool m_p2; + uint64_t m_t_b; + uint64_t m_not_used; + bool m_y_vel_sign; + uint64_t m_y_vel_value; + bool m_y_accel_sign; + uint64_t m_y_accel_value; + bool m_y_sign; + uint64_t m_y_value; + glonass_t* m__root; + glonass_t* m__parent; + + public: + uint64_t b_n() const { return m_b_n; } + bool p2() const { return m_p2; } + uint64_t t_b() const { return m_t_b; } + uint64_t not_used() const { return m_not_used; } + bool y_vel_sign() const { return m_y_vel_sign; } + uint64_t y_vel_value() const { return m_y_vel_value; } + bool y_accel_sign() const { return m_y_accel_sign; } + uint64_t y_accel_value() const { return m_y_accel_value; } + bool y_sign() const { return m_y_sign; } + uint64_t y_value() const { return m_y_value; } + glonass_t* _root() const { return m__root; } + glonass_t* _parent() const { return m__parent; } + }; + + class string_3_t : public kaitai::kstruct { + + public: + + string_3_t(kaitai::kstream* p__io, glonass_t* p__parent = 0, glonass_t* p__root = 0); + + private: + void _read(); + void _clean_up(); + + public: + ~string_3_t(); + + private: + bool f_gamma_n; + int32_t m_gamma_n; + + public: + int32_t gamma_n(); + + private: + bool f_z_vel; + int32_t m_z_vel; + + public: + int32_t z_vel(); + + private: + bool f_z_accel; + int32_t m_z_accel; + + public: + int32_t z_accel(); + + private: + bool f_z; + int32_t m_z; + + public: + int32_t z(); + + private: + bool m_p3; + bool m_gamma_n_sign; + uint64_t m_gamma_n_value; + bool m_not_used; + uint64_t m_p; + bool m_l_n; + bool m_z_vel_sign; + uint64_t m_z_vel_value; + bool m_z_accel_sign; + uint64_t m_z_accel_value; + bool m_z_sign; + uint64_t m_z_value; + glonass_t* m__root; + glonass_t* m__parent; + + public: + bool p3() const { return m_p3; } + bool gamma_n_sign() const { return m_gamma_n_sign; } + uint64_t gamma_n_value() const { return m_gamma_n_value; } + bool not_used() const { return m_not_used; } + uint64_t p() const { return m_p; } + bool l_n() const { return m_l_n; } + bool z_vel_sign() const { return m_z_vel_sign; } + uint64_t z_vel_value() const { return m_z_vel_value; } + bool z_accel_sign() const { return m_z_accel_sign; } + uint64_t z_accel_value() const { return m_z_accel_value; } + bool z_sign() const { return m_z_sign; } + uint64_t z_value() const { return m_z_value; } + glonass_t* _root() const { return m__root; } + glonass_t* _parent() const { return m__parent; } + }; + +private: + bool m_idle_chip; + uint64_t m_string_number; + kaitai::kstruct* m_data; + uint64_t m_hamming_code; + uint64_t m_pad_1; + uint64_t m_superframe_number; + uint64_t m_pad_2; + uint64_t m_frame_number; + glonass_t* m__root; + kaitai::kstruct* m__parent; + +public: + bool idle_chip() const { return m_idle_chip; } + uint64_t string_number() const { return m_string_number; } + kaitai::kstruct* data() const { return m_data; } + uint64_t hamming_code() const { return m_hamming_code; } + uint64_t pad_1() const { return m_pad_1; } + uint64_t superframe_number() const { return m_superframe_number; } + uint64_t pad_2() const { return m_pad_2; } + uint64_t frame_number() const { return m_frame_number; } + glonass_t* _root() const { return m__root; } + kaitai::kstruct* _parent() const { return m__parent; } +}; + +#endif // GLONASS_H_ diff --git a/selfdrive/locationd/generated/gps.cpp b/selfdrive/locationd/generated/gps.cpp index 9b020735bb..8e1cb85b95 100644 --- a/selfdrive/locationd/generated/gps.cpp +++ b/selfdrive/locationd/generated/gps.cpp @@ -274,9 +274,9 @@ gps_t::tlm_t::tlm_t(kaitai::kstream* p__io, gps_t* p__parent, gps_t* p__root) : } void gps_t::tlm_t::_read() { - m_magic = m__io->read_bytes(1); - if (!(magic() == std::string("\x8B", 1))) { - throw kaitai::validation_not_equal_error(std::string("\x8B", 1), magic(), _io(), std::string("/types/tlm/seq/0")); + m_preamble = m__io->read_bytes(1); + if (!(preamble() == std::string("\x8B", 1))) { + throw kaitai::validation_not_equal_error(std::string("\x8B", 1), preamble(), _io(), std::string("/types/tlm/seq/0")); } m_tlm = m__io->read_bits_int_be(14); m_integrity_status = m__io->read_bits_int_be(1); diff --git a/selfdrive/locationd/generated/gps.h b/selfdrive/locationd/generated/gps.h index 293e2e4a05..9dfc5031f5 100644 --- a/selfdrive/locationd/generated/gps.h +++ b/selfdrive/locationd/generated/gps.h @@ -273,7 +273,7 @@ public: ~tlm_t(); private: - std::string m_magic; + std::string m_preamble; uint64_t m_tlm; bool m_integrity_status; bool m_reserved; @@ -281,7 +281,7 @@ public: gps_t* m__parent; public: - std::string magic() const { return m_magic; } + std::string preamble() const { return m_preamble; } uint64_t tlm() const { return m_tlm; } bool integrity_status() const { return m_integrity_status; } bool reserved() const { return m_reserved; } diff --git a/selfdrive/locationd/generated/ubx.cpp b/selfdrive/locationd/generated/ubx.cpp index 5e743e1ee7..34fe1e52ca 100644 --- a/selfdrive/locationd/generated/ubx.cpp +++ b/selfdrive/locationd/generated/ubx.cpp @@ -89,13 +89,10 @@ void ubx_t::rxm_rawx_t::_read() { m_num_meas = m__io->read_u1(); m_rec_stat = m__io->read_u1(); m_reserved1 = m__io->read_bytes(3); - int l_measurements = num_meas(); m__raw_measurements = new std::vector(); - m__raw_measurements->reserve(l_measurements); m__io__raw_measurements = new std::vector(); - m__io__raw_measurements->reserve(l_measurements); m_measurements = new std::vector(); - m_measurements->reserve(l_measurements); + const int l_measurements = num_meas(); for (int i = 0; i < l_measurements; i++) { m__raw_measurements->push_back(m__io->read_bytes(32)); kaitai::kstream* io__raw_measurements = new kaitai::kstream(m__raw_measurements->at(m__raw_measurements->size() - 1)); @@ -184,9 +181,8 @@ void ubx_t::rxm_sfrbx_t::_read() { m_reserved2 = m__io->read_bytes(1); m_version = m__io->read_u1(); m_reserved3 = m__io->read_bytes(1); - int l_body = num_words(); m_body = new std::vector(); - m_body->reserve(l_body); + const int l_body = num_words(); for (int i = 0; i < l_body; i++) { m_body->push_back(m__io->read_u4le()); } diff --git a/selfdrive/locationd/glonass.ksy b/selfdrive/locationd/glonass.ksy new file mode 100644 index 0000000000..be99f6e497 --- /dev/null +++ b/selfdrive/locationd/glonass.ksy @@ -0,0 +1,176 @@ +# http://gauss.gge.unb.ca/GLONASS.ICD.pdf +# some variables are misprinted but good in the old doc +# https://www.unavco.org/help/glossary/docs/ICD_GLONASS_4.0_(1998)_en.pdf +meta: + id: glonass + endian: be + bit-endian: be +seq: + - id: idle_chip + type: b1 + - id: string_number + type: b4 + - id: data + type: + switch-on: string_number + cases: + 1: string_1 + 2: string_2 + 3: string_3 + 4: string_4 + 5: string_5 + _: string_non_immediate + - id: hamming_code + type: b8 + - id: pad_1 + type: b11 + - id: superframe_number + type: b16 + - id: pad_2 + type: b8 + - id: frame_number + type: b8 + +types: + string_1: + seq: + - id: not_used + type: b2 + - id: p1 + type: b2 + - id: t_k + type: b12 + - id: x_vel_sign + type: b1 + - id: x_vel_value + type: b23 + - id: x_accel_sign + type: b1 + - id: x_accel_value + type: b4 + - id: x_sign + type: b1 + - id: x_value + type: b26 + instances: + x_vel: + value: 'x_vel_sign ? (x_vel_value * (-1)) : x_vel_value' + x_accel: + value: 'x_accel_sign ? (x_accel_value * (-1)) : x_accel_value' + x: + value: 'x_sign ? (x_value * (-1)) : x_value' + string_2: + seq: + - id: b_n + type: b3 + - id: p2 + type: b1 + - id: t_b + type: b7 + - id: not_used + type: b5 + - id: y_vel_sign + type: b1 + - id: y_vel_value + type: b23 + - id: y_accel_sign + type: b1 + - id: y_accel_value + type: b4 + - id: y_sign + type: b1 + - id: y_value + type: b26 + instances: + y_vel: + value: 'y_vel_sign ? (y_vel_value * (-1)) : y_vel_value' + y_accel: + value: 'y_accel_sign ? (y_accel_value * (-1)) : y_accel_value' + y: + value: 'y_sign ? (y_value * (-1)) : y_value' + string_3: + seq: + - id: p3 + type: b1 + - id: gamma_n_sign + type: b1 + - id: gamma_n_value + type: b10 + - id: not_used + type: b1 + - id: p + type: b2 + - id: l_n + type: b1 + - id: z_vel_sign + type: b1 + - id: z_vel_value + type: b23 + - id: z_accel_sign + type: b1 + - id: z_accel_value + type: b4 + - id: z_sign + type: b1 + - id: z_value + type: b26 + instances: + gamma_n: + value: 'gamma_n_sign ? (gamma_n_value * (-1)) : gamma_n_value' + z_vel: + value: 'z_vel_sign ? (z_vel_value * (-1)) : z_vel_value' + z_accel: + value: 'z_accel_sign ? (z_accel_value * (-1)) : z_accel_value' + z: + value: 'z_sign ? (z_value * (-1)) : z_value' + string_4: + seq: + - id: tau_n_sign + type: b1 + - id: tau_n_value + type: b21 + - id: delta_tau_n_sign + type: b1 + - id: delta_tau_n_value + type: b4 + - id: e_n + type: b5 + - id: not_used_1 + type: b14 + - id: p4 + type: b1 + - id: f_t + type: b4 + - id: not_used_2 + type: b3 + - id: n_t + type: b11 + - id: n + type: b5 + - id: m + type: b2 + instances: + tau_n: + value: 'tau_n_sign ? (tau_n_value * (-1)) : tau_n_value' + delta_tau_n: + value: 'delta_tau_n_sign ? (delta_tau_n_value * (-1)) : delta_tau_n_value' + string_5: + seq: + - id: n_a + type: b11 + - id: tau_c + type: b32 + - id: not_used + type: b1 + - id: n_4 + type: b5 + - id: tau_gps + type: b22 + - id: l_n + type: b1 + string_non_immediate: + seq: + - id: data_1 + type: b64 + - id: data_2 + type: b8 diff --git a/selfdrive/locationd/glonass_fix.patch b/selfdrive/locationd/glonass_fix.patch new file mode 100644 index 0000000000..fa34a8ef15 --- /dev/null +++ b/selfdrive/locationd/glonass_fix.patch @@ -0,0 +1,13 @@ +diff --git a/selfdrive/locationd/generated/glonass.cpp b/selfdrive/locationd/generated/glonass.cpp +index 5b17bc327..b5c6aa610 100644 +--- a/selfdrive/locationd/generated/glonass.cpp ++++ b/selfdrive/locationd/generated/glonass.cpp +@@ -17,7 +17,7 @@ glonass_t::glonass_t(kaitai::kstream* p__io, kaitai::kstruct* p__parent, glonass + void glonass_t::_read() { + m_idle_chip = m__io->read_bits_int_be(1); + m_string_number = m__io->read_bits_int_be(4); +- m__io->align_to_byte(); ++ //m__io->align_to_byte(); + switch (string_number()) { + case 4: { + m_data = new string_4_t(m__io, this, m__root); diff --git a/selfdrive/locationd/gps.ksy b/selfdrive/locationd/gps.ksy index 6f5cde316b..893ad1b25b 100644 --- a/selfdrive/locationd/gps.ksy +++ b/selfdrive/locationd/gps.ksy @@ -19,7 +19,7 @@ seq: types: tlm: seq: - - id: magic + - id: preamble contents: [0x8b] - id: tlm type: b14 diff --git a/selfdrive/locationd/laikad.py b/selfdrive/locationd/laikad.py index b8d86c63fd..e85cd0ad42 100755 --- a/selfdrive/locationd/laikad.py +++ b/selfdrive/locationd/laikad.py @@ -17,7 +17,7 @@ from common.params import Params, put_nonblocking from laika import AstroDog from laika.constants import SECS_IN_HR, SECS_IN_MIN from laika.downloader import DownloadFailed -from laika.ephemeris import Ephemeris, EphemerisType, convert_ublox_ephem, parse_qcom_ephem +from laika.ephemeris import Ephemeris, EphemerisType, convert_ublox_gps_ephem, convert_ublox_glonass_ephem, parse_qcom_ephem from laika.gps_time import GPSTime from laika.helpers import ConstellationId from laika.raw_gnss import GNSSMeasurement, correct_measurements, process_measurements, read_raw_ublox, read_raw_qcom @@ -139,17 +139,22 @@ class Laikad: if self.use_qcom: return gnss_msg.which() == 'drSvPoly' else: - return gnss_msg.which() == 'ephemeris' + return gnss_msg.which() in ('ephemeris', 'glonassEphemeris') def read_ephemeris(self, gnss_msg): - # TODO this only works on GLONASS if self.use_qcom: # TODO this is not robust to gps week rollover if self.gps_week is None: return ephem = parse_qcom_ephem(gnss_msg.drSvPoly, self.gps_week) else: - ephem = convert_ublox_ephem(gnss_msg.ephemeris) + if gnss_msg.which() == 'ephemeris': + ephem = convert_ublox_gps_ephem(gnss_msg.ephemeris) + elif gnss_msg.which() == 'glonassEphemeris': + ephem = convert_ublox_glonass_ephem(gnss_msg.glonassEphemeris) + else: + cloudlog.error(f"Unsupported ephemeris type: {gnss_msg.which()}") + return self.astro_dog.add_navs({ephem.prn: [ephem]}) self.cache_ephemeris(t=ephem.epoch) @@ -410,8 +415,13 @@ def main(sm=None, pm=None, qc=None): if pm is None: pm = messaging.PubMaster(['gnssMeasurements']) + # disable until set as main gps source, to better analyze startup time + use_internet = False #"LAIKAD_NO_INTERNET" not in os.environ + replay = "REPLAY" in os.environ - use_internet = "LAIKAD_NO_INTERNET" not in os.environ + if replay or "CI" in os.environ: + use_internet = True + laikad = Laikad(save_ephemeris=not replay, auto_fetch_navs=use_internet, use_qcom=use_qcom) while True: diff --git a/selfdrive/locationd/locationd.cc b/selfdrive/locationd/locationd.cc index 928e19081e..8941b50248 100755 --- a/selfdrive/locationd/locationd.cc +++ b/selfdrive/locationd/locationd.cc @@ -354,6 +354,7 @@ void Localizer::handle_gps(double current_time, const cereal::GpsLocationData::R this->reset_kalman(NAN, initial_pose_ecef_quat, ecef_pos, ecef_vel, ecef_pos_R, ecef_vel_R); } + this->last_gps_msg = sensor_time; this->kf->predict_and_observe(sensor_time, OBSERVATION_ECEF_POS, { ecef_pos }, { ecef_pos_R }); this->kf->predict_and_observe(sensor_time, OBSERVATION_ECEF_VEL, { ecef_vel }, { ecef_vel_R }); } @@ -588,12 +589,12 @@ void Localizer::handle_msg(const cereal::Event::Reader& log) { this->handle_sensor(t, log.getAccelerometer()); } else if (log.isGyroscope()) { this->handle_sensor(t, log.getGyroscope()); - //} else if (log.isGpsLocation()) { - // this->handle_gps(t, log.getGpsLocation(), GPS_QUECTEL_SENSOR_TIME_OFFSET); - //} else if (log.isGpsLocationExternal()) { - // this->handle_gps(t, log.getGpsLocationExternal(), GPS_UBLOX_SENSOR_TIME_OFFSET); - } else if (log.isGnssMeasurements()) { - this->handle_gnss(t, log.getGnssMeasurements()); + } else if (log.isGpsLocation()) { + this->handle_gps(t, log.getGpsLocation(), GPS_QUECTEL_SENSOR_TIME_OFFSET); + } else if (log.isGpsLocationExternal()) { + this->handle_gps(t, log.getGpsLocationExternal(), GPS_UBLOX_SENSOR_TIME_OFFSET); + //} else if (log.isGnssMeasurements()) { + // this->handle_gnss(t, log.getGnssMeasurements()); } else if (log.isCarState()) { this->handle_car_state(t, log.getCarState()); } else if (log.isCameraOdometry()) { @@ -657,11 +658,17 @@ void Localizer::determine_gps_mode(double current_time) { int Localizer::locationd_thread() { ublox_available = Params().getBool("UbloxAvailable", true); - const std::initializer_list service_list = {"gnssMeasurements", "cameraOdometry", "liveCalibration", + const char* gps_location_socket; + if (ublox_available) { + gps_location_socket = "gpsLocationExternal"; + } else { + gps_location_socket = "gpsLocation"; + } + const std::initializer_list service_list = {gps_location_socket, "cameraOdometry", "liveCalibration", "carState", "carParams", "accelerometer", "gyroscope"}; // TODO: remove carParams once we're always sending at 100Hz - SubMaster sm(service_list, {}, nullptr, {"gnssMeasurements", "carParams"}); + SubMaster sm(service_list, {}, nullptr, {gps_location_socket, "carParams"}); PubMaster pm({"liveLocationKalman"}); uint64_t cnt = 0; diff --git a/selfdrive/locationd/models/lane_kf.py b/selfdrive/locationd/models/lane_kf.py new file mode 100755 index 0000000000..4d38fa8e09 --- /dev/null +++ b/selfdrive/locationd/models/lane_kf.py @@ -0,0 +1,105 @@ +#!/usr/bin/env python3 +import sys +import numpy as np +import sympy as sp + +from selfdrive.locationd.models.constants import ObservationKind +from rednose.helpers.ekf_sym import gen_code, EKF_sym + + +class LaneKalman(): + name = 'lane' + + @staticmethod + def generate_code(generated_dir): + # make functions and jacobians with sympy + # state variables + dim = 6 + state = sp.MatrixSymbol('state', dim, 1) + + dd = sp.Symbol('dd') # WARNING: NOT TIME + + # Time derivative of the state as a function of state + state_dot = sp.Matrix(np.zeros((dim, 1))) + state_dot[:3,0] = sp.Matrix(state[3:6,0]) + + # Basic descretization, 1st order intergrator + # Can be pretty bad if dt is big + f_sym = sp.Matrix(state) + dd*state_dot + + # + # Observation functions + # + h_lane_sym = sp.Matrix(state[:3,0]) + obs_eqs = [[h_lane_sym, ObservationKind.LANE_PT, None]] + gen_code(generated_dir, LaneKalman.name, f_sym, dd, state, obs_eqs, dim, dim) + + def __init__(self, generated_dir, pt_std=5): + # state + # left and right lane centers in ecef + # WARNING: this is not a temporal model + # the 'time' in this kalman filter is + # the distance traveled by the vehicle, + # which should approximately be the + # distance along the lane path + # a more logical parametrization + # states 0-2 are ecef coordinates distance d + # states 3-5 is the 3d "velocity" of the + # lane in ecef (m/m). + x_initial = np.array([0,0,0, + 0,0,0]) + + # state covariance + P_initial = np.diag([1e16, 1e16, 1e16, + 1**2, 1**2, 1**2]) + + # process noise + Q = np.diag([0.1**2, 0.1**2, 0.1**2, + 0.1**2, 0.1**2, 0.1*2]) + + self.dim_state = len(x_initial) + + # init filter + self.filter = EKF_sym(generated_dir, self.name, Q, x_initial, P_initial, x_initial.shape[0], P_initial.shape[0]) + self.obs_noise = {ObservationKind.LANE_PT: np.diag([pt_std**2]*3)} + + @property + def x(self): + return self.filter.state() + + @property + def P(self): + return self.filter.covs() + + def predict(self, t): + return self.filter.predict(t) + + def rts_smooth(self, estimates): + return self.filter.rts_smooth(estimates, norm_quats=False) + + + def init_state(self, state, covs_diag=None, covs=None, filter_time=None): + if covs_diag is not None: + P = np.diag(covs_diag) + elif covs is not None: + P = covs + else: + P = self.filter.covs() + self.filter.init_state(state, P, filter_time) + + def predict_and_observe(self, t, kind, data): + data = np.atleast_2d(data) + return self.filter.predict_and_update_batch(t, kind, data, self.get_R(kind, len(data))) + + def get_R(self, kind, n): + obs_noise = self.obs_noise[kind] + dim = obs_noise.shape[0] + R = np.zeros((n, dim, dim)) + for i in range(n): + R[i,:,:] = obs_noise + return R + + +if __name__ == "__main__": + generated_dir = sys.argv[2] + LaneKalman.generate_code(generated_dir) diff --git a/selfdrive/locationd/paramsd.py b/selfdrive/locationd/paramsd.py index 7e7c2b091f..7e30b1e3a7 100755 --- a/selfdrive/locationd/paramsd.py +++ b/selfdrive/locationd/paramsd.py @@ -14,8 +14,9 @@ from system.swaglog import cloudlog MAX_ANGLE_OFFSET_DELTA = 20 * DT_MDL # Max 20 deg/s -ROLL_MAX_DELTA = np.radians(20.0) * DT_MDL # 20deg in 1 second is well within curvature limits +ROLL_MAX_DELTA = math.radians(20.0) * DT_MDL # 20deg in 1 second is well within curvature limits ROLL_MIN, ROLL_MAX = math.radians(-10), math.radians(10) +ROLL_STD_MAX = math.radians(1.5) LATERAL_ACC_SENSOR_THRESHOLD = 4.0 @@ -37,8 +38,7 @@ class ParamsLearner: self.yaw_rate_std = 0.0 self.roll = 0.0 self.steering_angle = 0.0 - - self.valid = True + self.roll_valid = False def handle_log(self, t, which, msg): if which == 'liveLocationKalman': @@ -47,8 +47,8 @@ class ParamsLearner: localizer_roll = msg.orientationNED.value[0] localizer_roll_std = np.radians(1) if np.isnan(msg.orientationNED.std[0]) else msg.orientationNED.std[0] - roll_valid = msg.orientationNED.valid and ROLL_MIN < localizer_roll < ROLL_MAX - if roll_valid: + self.roll_valid = (localizer_roll_std < ROLL_STD_MAX) and (ROLL_MIN < localizer_roll < ROLL_MAX) and msg.sensorsOK + if self.roll_valid: roll = localizer_roll # Experimentally found multiplier of 2 to be best trade-off between stability and accuracy or similar? roll_std = 2 * localizer_roll_std @@ -156,6 +156,7 @@ def main(sm=None, pm=None): learner = ParamsLearner(CP, params['steerRatio'], params['stiffnessFactor'], math.radians(params['angleOffsetAverageDeg'])) angle_offset_average = params['angleOffsetAverageDeg'] angle_offset = angle_offset_average + roll = 0.0 while True: sm.update() @@ -175,6 +176,8 @@ def main(sm=None, pm=None): angle_offset_average = clip(math.degrees(x[States.ANGLE_OFFSET]), angle_offset_average - MAX_ANGLE_OFFSET_DELTA, angle_offset_average + MAX_ANGLE_OFFSET_DELTA) angle_offset = clip(math.degrees(x[States.ANGLE_OFFSET] + x[States.ANGLE_OFFSET_FAST]), angle_offset - MAX_ANGLE_OFFSET_DELTA, angle_offset + MAX_ANGLE_OFFSET_DELTA) + roll = clip(float(x[States.ROAD_ROLL]), roll - ROLL_MAX_DELTA, roll + ROLL_MAX_DELTA) + roll_std = float(P[States.ROAD_ROLL]) # Account for the opposite signs of the yaw rates sensors_valid = bool(abs(learner.speed * (x[States.YAW_RATE] + learner.yaw_rate)) < LATERAL_ACC_SENSOR_THRESHOLD) @@ -185,12 +188,14 @@ def main(sm=None, pm=None): liveParameters.sensorValid = sensors_valid liveParameters.steerRatio = float(x[States.STEER_RATIO]) liveParameters.stiffnessFactor = float(x[States.STIFFNESS]) - liveParameters.roll = float(x[States.ROAD_ROLL]) + liveParameters.roll = roll liveParameters.angleOffsetAverageDeg = angle_offset_average liveParameters.angleOffsetDeg = angle_offset liveParameters.valid = all(( abs(liveParameters.angleOffsetAverageDeg) < 10.0, abs(liveParameters.angleOffsetDeg) < 10.0, + abs(liveParameters.roll) < ROLL_MAX, + roll_std < ROLL_STD_MAX, 0.2 <= liveParameters.stiffnessFactor <= 5.0, min_sr <= liveParameters.steerRatio <= max_sr, )) diff --git a/selfdrive/locationd/test/test_glonass_kaitai.cc b/selfdrive/locationd/test/test_glonass_kaitai.cc new file mode 100644 index 0000000000..22f5202a3d --- /dev/null +++ b/selfdrive/locationd/test/test_glonass_kaitai.cc @@ -0,0 +1,360 @@ +#include +#include +#include +#include +#include +#include + +#include "catch2/catch.hpp" +#include "selfdrive/locationd/generated/glonass.h" + +typedef std::vector> string_data; + +#define IDLE_CHIP_IDX 0 +#define STRING_NUMBER_IDX 1 +// string data 1-5 +#define HC_IDX 0 +#define PAD1_IDX 1 +#define SUPERFRAME_IDX 2 +#define PAD2_IDX 3 +#define FRAME_IDX 4 + +// Indexes for string number 1 +#define ST1_NU_IDX 2 +#define ST1_P1_IDX 3 +#define ST1_T_K_IDX 4 +#define ST1_X_VEL_S_IDX 5 +#define ST1_X_VEL_V_IDX 6 +#define ST1_X_ACCEL_S_IDX 7 +#define ST1_X_ACCEL_V_IDX 8 +#define ST1_X_S_IDX 9 +#define ST1_X_V_IDX 10 +#define ST1_HC_OFF 11 + +// Indexes for string number 2 +#define ST2_BN_IDX 2 +#define ST2_P2_IDX 3 +#define ST2_TB_IDX 4 +#define ST2_NU_IDX 5 +#define ST2_Y_VEL_S_IDX 6 +#define ST2_Y_VEL_V_IDX 7 +#define ST2_Y_ACCEL_S_IDX 8 +#define ST2_Y_ACCEL_V_IDX 9 +#define ST2_Y_S_IDX 10 +#define ST2_Y_V_IDX 11 +#define ST2_HC_OFF 12 + +// Indexes for string number 3 +#define ST3_P3_IDX 2 +#define ST3_GAMMA_N_S_IDX 3 +#define ST3_GAMMA_N_V_IDX 4 +#define ST3_NU_1_IDX 5 +#define ST3_P_IDX 6 +#define ST3_L_N_IDX 7 +#define ST3_Z_VEL_S_IDX 8 +#define ST3_Z_VEL_V_IDX 9 +#define ST3_Z_ACCEL_S_IDX 10 +#define ST3_Z_ACCEL_V_IDX 11 +#define ST3_Z_S_IDX 12 +#define ST3_Z_V_IDX 13 +#define ST3_HC_OFF 14 + +// Indexes for string number 4 +#define ST4_TAU_N_S_IDX 2 +#define ST4_TAU_N_V_IDX 3 +#define ST4_DELTA_TAU_N_S_IDX 4 +#define ST4_DELTA_TAU_N_V_IDX 5 +#define ST4_E_N_IDX 6 +#define ST4_NU_1_IDX 7 +#define ST4_P4_IDX 8 +#define ST4_F_T_IDX 9 +#define ST4_NU_2_IDX 10 +#define ST4_N_T_IDX 11 +#define ST4_N_IDX 12 +#define ST4_M_IDX 13 +#define ST4_HC_OFF 14 + +// Indexes for string number 5 +#define ST5_N_A_IDX 2 +#define ST5_TAU_C_IDX 3 +#define ST5_NU_IDX 4 +#define ST5_N_4_IDX 5 +#define ST5_TAU_GPS_IDX 6 +#define ST5_L_N_IDX 7 +#define ST5_HC_OFF 8 + +// Indexes for non immediate +#define ST6_DATA_1_IDX 2 +#define ST6_DATA_2_IDX 3 +#define ST6_HC_OFF 4 + + +std::string generate_inp_data(string_data& data) { + std::string inp_data = ""; + for (auto& [b, v] : data) { + std::string tmp = std::bitset<64>(v).to_string(); + inp_data += tmp.substr(64-b, b); + } + assert(inp_data.size() == 128); + + std::string string_data; + string_data.reserve(16); + for (int i = 0; i < 128; i+=8) { + std::string substr = inp_data.substr(i, 8); + string_data.push_back( (uint8_t)std::stoi(substr.c_str(), 0, 2)); + } + + return string_data; +} + +string_data generate_string_data(uint8_t string_number) { + + srand((unsigned)time(0)); + string_data data; // + data.push_back({1, 0}); // idle chip + data.push_back({4, string_number}); // string number + + if (string_number == 1) { + data.push_back({2, 3}); // not_used + data.push_back({2, 1}); // p1 + data.push_back({12, 113}); // t_k + data.push_back({1, rand() & 1}); // x_vel_sign + data.push_back({23, 7122}); // x_vel_value + data.push_back({1, rand() & 1}); // x_accel_sign + data.push_back({4, 3}); // x_accel_value + data.push_back({1, rand() & 1}); // x_sign + data.push_back({26, 33554431}); // x_value + } else if (string_number == 2) { + data.push_back({3, 3}); // b_n + data.push_back({1, 1}); // p2 + data.push_back({7, 123}); // t_b + data.push_back({5, 31}); // not_used + data.push_back({1, rand() & 1}); // y_vel_sign + data.push_back({23, 7422}); // y_vel_value + data.push_back({1, rand() & 1}); // y_accel_sign + data.push_back({4, 3}); // y_accel_value + data.push_back({1, rand() & 1}); // y_sign + data.push_back({26, 67108863}); // y_value + } else if (string_number == 3) { + data.push_back({1, 0}); // p3 + data.push_back({1, 1}); // gamma_n_sign + data.push_back({10, 123}); // gamma_n_value + data.push_back({1, 0}); // not_used + data.push_back({2, 2}); // p + data.push_back({1, 1}); // l_n + data.push_back({1, rand() & 1}); // z_vel_sign + data.push_back({23, 1337}); // z_vel_value + data.push_back({1, rand() & 1}); // z_accel_sign + data.push_back({4, 9}); // z_accel_value + data.push_back({1, rand() & 1}); // z_sign + data.push_back({26, 100023}); // z_value + } else if (string_number == 4) { + data.push_back({1, rand() & 1}); // tau_n_sign + data.push_back({21, 197152}); // tau_n_value + data.push_back({1, rand() & 1}); // delta_tau_n_sign + data.push_back({4, 4}); // delta_tau_n_value + data.push_back({5, 0}); // e_n + data.push_back({14, 2}); // not_used_1 + data.push_back({1, 1}); // p4 + data.push_back({4, 9}); // f_t + data.push_back({3, 3}); // not_used_2 + data.push_back({11, 2047}); // n_t + data.push_back({5, 2}); // n + data.push_back({2, 1}); // m + } else if (string_number == 5) { + data.push_back({11, 2047}); // n_a + data.push_back({32, 4294767295}); // tau_c + data.push_back({1, 0}); // not_used_1 + data.push_back({5, 2}); // n_4 + data.push_back({22, 4114304}); // tau_gps + data.push_back({1, 0}); // l_n + } else { // non-immediate data is not parsed + data.push_back({64, rand()}); // data_1 + data.push_back({8, 6}); // data_2 + } + + data.push_back({8, rand() & 0xFF}); // hamming code + data.push_back({11, rand() & 0x7FF}); // pad + data.push_back({16, rand() & 0xFFFF}); // superframe + data.push_back({8, rand() & 0xFF}); // pad + data.push_back({8, rand() & 0xFF}); // frame + return data; +} + +TEST_CASE("parse_string_number_1"){ + string_data data = generate_string_data(1); + std::string inp_data = generate_inp_data(data); + + kaitai::kstream stream(inp_data); + glonass_t gl_string(&stream); + + REQUIRE(gl_string.idle_chip() == data[IDLE_CHIP_IDX].second); + REQUIRE(gl_string.string_number() == data[STRING_NUMBER_IDX].second); + REQUIRE(gl_string.hamming_code() == data[ST1_HC_OFF + HC_IDX].second); + REQUIRE(gl_string.pad_1() == data[ST1_HC_OFF + PAD1_IDX].second); + REQUIRE(gl_string.superframe_number() == data[ST1_HC_OFF + SUPERFRAME_IDX].second); + REQUIRE(gl_string.pad_2() == data[ST1_HC_OFF + PAD2_IDX].second); + REQUIRE(gl_string.frame_number() == data[ST1_HC_OFF + FRAME_IDX].second); + + kaitai::kstream str1(inp_data); + glonass_t str1_data(&str1); + glonass_t::string_1_t* s1 = static_cast(str1_data.data()); + + REQUIRE(s1->not_used() == data[ST1_NU_IDX].second); + REQUIRE(s1->p1() == data[ST1_P1_IDX].second); + REQUIRE(s1->t_k() == data[ST1_T_K_IDX].second); + + int mul = s1->x_vel_sign() ? (-1) : 1; + REQUIRE(s1->x_vel() == (data[ST1_X_VEL_V_IDX].second * mul)); + mul = s1->x_accel_sign() ? (-1) : 1; + REQUIRE(s1->x_accel() == (data[ST1_X_ACCEL_V_IDX].second * mul)); + mul = s1->x_sign() ? (-1) : 1; + REQUIRE(s1->x() == (data[ST1_X_V_IDX].second * mul)); +} + +TEST_CASE("parse_string_number_2"){ + string_data data = generate_string_data(2); + std::string inp_data = generate_inp_data(data); + + kaitai::kstream stream(inp_data); + glonass_t gl_string(&stream); + + REQUIRE(gl_string.idle_chip() == data[IDLE_CHIP_IDX].second); + REQUIRE(gl_string.string_number() == data[STRING_NUMBER_IDX].second); + REQUIRE(gl_string.hamming_code() == data[ST2_HC_OFF + HC_IDX].second); + REQUIRE(gl_string.pad_1() == data[ST2_HC_OFF + PAD1_IDX].second); + REQUIRE(gl_string.superframe_number() == data[ST2_HC_OFF + SUPERFRAME_IDX].second); + REQUIRE(gl_string.pad_2() == data[ST2_HC_OFF + PAD2_IDX].second); + REQUIRE(gl_string.frame_number() == data[ST2_HC_OFF + FRAME_IDX].second); + + kaitai::kstream str2(inp_data); + glonass_t str2_data(&str2); + glonass_t::string_2_t* s2 = static_cast(str2_data.data()); + + REQUIRE(s2->b_n() == data[ST2_BN_IDX].second); + REQUIRE(s2->not_used() == data[ST2_NU_IDX].second); + REQUIRE(s2->p2() == data[ST2_P2_IDX].second); + REQUIRE(s2->t_b() == data[ST2_TB_IDX].second); + int mul = s2->y_vel_sign() ? (-1) : 1; + REQUIRE(s2->y_vel() == (data[ST2_Y_VEL_V_IDX].second * mul)); + mul = s2->y_accel_sign() ? (-1) : 1; + REQUIRE(s2->y_accel() == (data[ST2_Y_ACCEL_V_IDX].second * mul)); + mul = s2->y_sign() ? (-1) : 1; + REQUIRE(s2->y() == (data[ST2_Y_V_IDX].second * mul)); +} + +TEST_CASE("parse_string_number_3"){ + string_data data = generate_string_data(3); + std::string inp_data = generate_inp_data(data); + + kaitai::kstream stream(inp_data); + glonass_t gl_string(&stream); + + REQUIRE(gl_string.idle_chip() == data[IDLE_CHIP_IDX].second); + REQUIRE(gl_string.string_number() == data[STRING_NUMBER_IDX].second); + REQUIRE(gl_string.hamming_code() == data[ST3_HC_OFF + HC_IDX].second); + REQUIRE(gl_string.pad_1() == data[ST3_HC_OFF + PAD1_IDX].second); + REQUIRE(gl_string.superframe_number() == data[ST3_HC_OFF + SUPERFRAME_IDX].second); + REQUIRE(gl_string.pad_2() == data[ST3_HC_OFF + PAD2_IDX].second); + REQUIRE(gl_string.frame_number() == data[ST3_HC_OFF + FRAME_IDX].second); + + kaitai::kstream str3(inp_data); + glonass_t str3_data(&str3); + glonass_t::string_3_t* s3 = static_cast(str3_data.data()); + + REQUIRE(s3->p3() == data[ST3_P3_IDX].second); + int mul = s3->gamma_n_sign() ? (-1) : 1; + REQUIRE(s3->gamma_n() == (data[ST3_GAMMA_N_V_IDX].second * mul)); + REQUIRE(s3->not_used() == data[ST3_NU_1_IDX].second); + REQUIRE(s3->p() == data[ST3_P_IDX].second); + REQUIRE(s3->l_n() == data[ST3_L_N_IDX].second); + mul = s3->z_vel_sign() ? (-1) : 1; + REQUIRE(s3->z_vel() == (data[ST3_Z_VEL_V_IDX].second * mul)); + mul = s3->z_accel_sign() ? (-1) : 1; + REQUIRE(s3->z_accel() == (data[ST3_Z_ACCEL_V_IDX].second * mul)); + mul = s3->z_sign() ? (-1) : 1; + REQUIRE(s3->z() == (data[ST3_Z_V_IDX].second * mul)); +} + +TEST_CASE("parse_string_number_4"){ + string_data data = generate_string_data(4); + std::string inp_data = generate_inp_data(data); + + kaitai::kstream stream(inp_data); + glonass_t gl_string(&stream); + + REQUIRE(gl_string.idle_chip() == data[IDLE_CHIP_IDX].second); + REQUIRE(gl_string.string_number() == data[STRING_NUMBER_IDX].second); + REQUIRE(gl_string.hamming_code() == data[ST4_HC_OFF + HC_IDX].second); + REQUIRE(gl_string.pad_1() == data[ST4_HC_OFF + PAD1_IDX].second); + REQUIRE(gl_string.superframe_number() == data[ST4_HC_OFF + SUPERFRAME_IDX].second); + REQUIRE(gl_string.pad_2() == data[ST4_HC_OFF + PAD2_IDX].second); + REQUIRE(gl_string.frame_number() == data[ST4_HC_OFF + FRAME_IDX].second); + + kaitai::kstream str4(inp_data); + glonass_t str4_data(&str4); + glonass_t::string_4_t* s4 = static_cast(str4_data.data()); + + int mul = s4->tau_n_sign() ? (-1) : 1; + REQUIRE(s4->tau_n() == (data[ST4_TAU_N_V_IDX].second * mul)); + mul = s4->delta_tau_n_sign() ? (-1) : 1; + REQUIRE(s4->delta_tau_n() == (data[ST4_DELTA_TAU_N_V_IDX].second * mul)); + REQUIRE(s4->e_n() == data[ST4_E_N_IDX].second); + REQUIRE(s4->not_used_1() == data[ST4_NU_1_IDX].second); + REQUIRE(s4->p4() == data[ST4_P4_IDX].second); + REQUIRE(s4->f_t() == data[ST4_F_T_IDX].second); + REQUIRE(s4->not_used_2() == data[ST4_NU_2_IDX].second); + REQUIRE(s4->n_t() == data[ST4_N_T_IDX].second); + REQUIRE(s4->n() == data[ST4_N_IDX].second); + REQUIRE(s4->m() == data[ST4_M_IDX].second); +} + +TEST_CASE("parse_string_number_5"){ + string_data data = generate_string_data(5); + std::string inp_data = generate_inp_data(data); + + kaitai::kstream stream(inp_data); + glonass_t gl_string(&stream); + + REQUIRE(gl_string.idle_chip() == data[IDLE_CHIP_IDX].second); + REQUIRE(gl_string.string_number() == data[STRING_NUMBER_IDX].second); + REQUIRE(gl_string.hamming_code() == data[ST5_HC_OFF + HC_IDX].second); + REQUIRE(gl_string.pad_1() == data[ST5_HC_OFF + PAD1_IDX].second); + REQUIRE(gl_string.superframe_number() == data[ST5_HC_OFF + SUPERFRAME_IDX].second); + REQUIRE(gl_string.pad_2() == data[ST5_HC_OFF + PAD2_IDX].second); + REQUIRE(gl_string.frame_number() == data[ST5_HC_OFF + FRAME_IDX].second); + + kaitai::kstream str5(inp_data); + glonass_t str5_data(&str5); + glonass_t::string_5_t* s5 = static_cast(str5_data.data()); + + REQUIRE(s5->n_a() == data[ST5_N_A_IDX].second); + REQUIRE(s5->tau_c() == data[ST5_TAU_C_IDX].second); + REQUIRE(s5->not_used() == data[ST5_NU_IDX].second); + REQUIRE(s5->n_4() == data[ST5_N_4_IDX].second); + REQUIRE(s5->tau_gps() == data[ST5_TAU_GPS_IDX].second); + REQUIRE(s5->l_n() == data[ST5_L_N_IDX].second); +} + +TEST_CASE("parse_string_number_NI"){ + string_data data = generate_string_data((rand() % 10) + 6); + std::string inp_data = generate_inp_data(data); + + kaitai::kstream stream(inp_data); + glonass_t gl_string(&stream); + + REQUIRE(gl_string.idle_chip() == data[IDLE_CHIP_IDX].second); + REQUIRE(gl_string.string_number() == data[STRING_NUMBER_IDX].second); + REQUIRE(gl_string.hamming_code() == data[ST6_HC_OFF + HC_IDX].second); + REQUIRE(gl_string.pad_1() == data[ST6_HC_OFF + PAD1_IDX].second); + REQUIRE(gl_string.superframe_number() == data[ST6_HC_OFF + SUPERFRAME_IDX].second); + REQUIRE(gl_string.pad_2() == data[ST6_HC_OFF + PAD2_IDX].second); + REQUIRE(gl_string.frame_number() == data[ST6_HC_OFF + FRAME_IDX].second); + + kaitai::kstream strni(inp_data); + glonass_t strni_data(&strni); + glonass_t::string_non_immediate_t* sni = static_cast(strni_data.data()); + + REQUIRE(sni->data_1() == data[ST6_DATA_1_IDX].second); + REQUIRE(sni->data_2() == data[ST6_DATA_2_IDX].second); +} diff --git a/selfdrive/locationd/test/test_glonass_runner.cc b/selfdrive/locationd/test/test_glonass_runner.cc new file mode 100644 index 0000000000..62bf7476a1 --- /dev/null +++ b/selfdrive/locationd/test/test_glonass_runner.cc @@ -0,0 +1,2 @@ +#define CATCH_CONFIG_MAIN +#include "catch2/catch.hpp" diff --git a/selfdrive/locationd/test/test_laikad.py b/selfdrive/locationd/test/test_laikad.py index 6059eab68f..d89f521228 100755 --- a/selfdrive/locationd/test/test_laikad.py +++ b/selfdrive/locationd/test/test_laikad.py @@ -22,7 +22,24 @@ def get_log(segs=range(0)): logs = [] for i in segs: logs.extend(LogReader(get_url("4cf7a6ad03080c90|2021-09-29--13-46-36", i))) - return [m for m in logs if m.which() == 'ubloxGnss'] + + all_logs = [m for m in logs if m.which() == 'ubloxGnss'] + low_gnss = [] + for m in all_logs: + if m.ubloxGnss.which() != 'measurementReport': + continue + + MAX_MEAS = 7 + if m.ubloxGnss.measurementReport.numMeas > MAX_MEAS: + mb = m.as_builder() + mb.ubloxGnss.measurementReport.numMeas = MAX_MEAS + mb.ubloxGnss.measurementReport.measurements = list(m.ubloxGnss.measurementReport.measurements)[:MAX_MEAS] + mb.ubloxGnss.measurementReport.measurements[0].pseudorange += 1000 + low_gnss.append(mb.as_reader()) + else: + low_gnss.append(m) + + return all_logs, low_gnss def verify_messages(lr, laikad, return_one_success=False): @@ -59,8 +76,9 @@ class TestLaikad(unittest.TestCase): @classmethod def setUpClass(cls): - logs = get_log(range(1)) + logs, low_gnss = get_log(range(1)) cls.logs = logs + cls.low_gnss = low_gnss first_gps_time = get_first_gps_time(logs) cls.first_gps_time = first_gps_time @@ -254,6 +272,18 @@ class TestLaikad(unittest.TestCase): # Verify orbit data is not downloaded mock_method.assert_not_called() + def test_low_gnss_meas(self): + cnt = 0 + laikad = Laikad() + for m in self.low_gnss: + msg = laikad.process_gnss_msg(m.ubloxGnss, m.logMonoTime, block=True) + if msg is None: + continue + gm = msg.gnssMeasurements + if len(gm.correctedMeasurements) != 0 and gm.positionECEF.valid: + cnt += 1 + self.assertEqual(cnt, 554) + def dict_has_values(self, dct): self.assertGreater(len(dct), 0) self.assertGreater(min([len(v) for v in dct.values()]), 0) diff --git a/selfdrive/locationd/test/test_locationd.py b/selfdrive/locationd/test/test_locationd.py index 6e65acaaf9..9f643e2b8f 100755 --- a/selfdrive/locationd/test/test_locationd.py +++ b/selfdrive/locationd/test/test_locationd.py @@ -15,7 +15,7 @@ from selfdrive.manager.process_config import managed_processes class TestLocationdProc(unittest.TestCase): MAX_WAITS = 1000 - LLD_MSGS = ['gnssMeasurements', 'cameraOdometry', 'carState', 'liveCalibration', + LLD_MSGS = ['gpsLocationExternal', 'cameraOdometry', 'carState', 'liveCalibration', 'accelerometer', 'gyroscope', 'magnetometer'] def setUp(self): @@ -46,14 +46,25 @@ class TestLocationdProc(unittest.TestCase): except capnp.lib.capnp.KjException: msg = messaging.new_message(name, 0) - if name == "gnssMeasurements": - msg.gnssMeasurements.measTime = t - msg.gnssMeasurements.positionECEF.value = [self.x , self.y, self.z] - msg.gnssMeasurements.positionECEF.std = [0,0,0] - msg.gnssMeasurements.positionECEF.valid = True - msg.gnssMeasurements.velocityECEF.value = [] - msg.gnssMeasurements.velocityECEF.std = [0,0,0] - msg.gnssMeasurements.velocityECEF.valid = True + + if name == "gpsLocationExternal": + msg.gpsLocationExternal.flags = 1 + msg.gpsLocationExternal.verticalAccuracy = 1.0 + msg.gpsLocationExternal.speedAccuracy = 1.0 + msg.gpsLocationExternal.bearingAccuracyDeg = 1.0 + msg.gpsLocationExternal.vNED = [0.0, 0.0, 0.0] + msg.gpsLocationExternal.latitude = float(self.lat) + msg.gpsLocationExternal.longitude = float(self.lon) + msg.gpsLocationExternal.unixTimestampMillis = t * 1e6 + msg.gpsLocationExternal.altitude = float(self.alt) + #if name == "gnssMeasurements": + # msg.gnssMeasurements.measTime = t + # msg.gnssMeasurements.positionECEF.value = [self.x , self.y, self.z] + # msg.gnssMeasurements.positionECEF.std = [0,0,0] + # msg.gnssMeasurements.positionECEF.valid = True + # msg.gnssMeasurements.velocityECEF.value = [] + # msg.gnssMeasurements.velocityECEF.std = [0,0,0] + # msg.gnssMeasurements.velocityECEF.valid = True elif name == 'cameraOdometry': msg.cameraOdometry.rot = [0.0, 0.0, 0.0] msg.cameraOdometry.rotStd = [0.0, 0.0, 0.0] @@ -84,9 +95,9 @@ class TestLocationdProc(unittest.TestCase): time.sleep(1) # wait for async params write lastGPS = json.loads(Params().get('LastGPSPosition')) - self.assertAlmostEqual(lastGPS['latitude'], self.lat, places=4) - self.assertAlmostEqual(lastGPS['longitude'], self.lon, places=4) - self.assertAlmostEqual(lastGPS['altitude'], self.alt, places=4) + self.assertAlmostEqual(lastGPS['latitude'], self.lat, places=3) + self.assertAlmostEqual(lastGPS['longitude'], self.lon, places=3) + self.assertAlmostEqual(lastGPS['altitude'], self.alt, places=3) if __name__ == "__main__": diff --git a/selfdrive/locationd/test/test_ublox_processing.py b/selfdrive/locationd/test/test_ublox_processing.py index 7aa588d43e..cd4ce0de04 100755 --- a/selfdrive/locationd/test/test_ublox_processing.py +++ b/selfdrive/locationd/test/test_ublox_processing.py @@ -1,5 +1,5 @@ import unittest - +import time import numpy as np from laika import AstroDog @@ -8,7 +8,8 @@ from laika.raw_gnss import correct_measurements, process_measurements, read_raw_ from laika.opt import calc_pos_fix from selfdrive.test.openpilotci import get_url from tools.lib.logreader import LogReader - +from selfdrive.test.helpers import with_processes +import cereal.messaging as messaging def get_gnss_measurements(log_reader): gnss_measurements = [] @@ -21,6 +22,12 @@ def get_gnss_measurements(log_reader): gnss_measurements.append(read_raw_ublox(report)) return gnss_measurements +def get_ublox_raw(log_reader): + ublox_raw = [] + for msg in log_reader: + if msg.which() == "ubloxRaw": + ublox_raw.append(msg) + return ublox_raw class TestUbloxProcessing(unittest.TestCase): NUM_TEST_PROCESS_MEAS = 10 @@ -30,6 +37,10 @@ class TestUbloxProcessing(unittest.TestCase): lr = LogReader(get_url("4cf7a6ad03080c90|2021-09-29--13-46-36", 0)) cls.gnss_measurements = get_gnss_measurements(lr) + # test gps ephemeris continuity check (drive has ephemeris issues with cutover data) + lr = LogReader(get_url("37b6542f3211019a|2023-01-15--23-45-10", 14)) + cls.ublox_raw = get_ublox_raw(lr) + def test_read_ublox_raw(self): count_gps = 0 count_glonass = 0 @@ -76,6 +87,29 @@ class TestUbloxProcessing(unittest.TestCase): self.assertEqual(count_processed_measurements, 69) self.assertEqual(count_corrected_measurements, 69) + @with_processes(['ubloxd']) + def test_ublox_gps_cutover(self): + time.sleep(2) + ugs = messaging.sub_sock("ubloxGnss", timeout=0.1) + ur_pm = messaging.PubMaster(['ubloxRaw']) + + def replay_segment(): + rcv_msgs = [] + for msg in self.ublox_raw: + ur_pm.send(msg.which(), msg.as_builder()) + time.sleep(0.01) + rcv_msgs += messaging.drain_sock(ugs) + + time.sleep(0.1) + rcv_msgs += messaging.drain_sock(ugs) + return rcv_msgs + + # replay twice to enforce cutover data on rewind + rcv_msgs = replay_segment() + rcv_msgs += replay_segment() + + ephems_cnt = sum(m.ubloxGnss.which() == 'ephemeris' for m in rcv_msgs) + self.assertEqual(ephems_cnt, 15) if __name__ == "__main__": unittest.main() diff --git a/selfdrive/locationd/torqued.py b/selfdrive/locationd/torqued.py index 588bca1578..fcc068d34e 100755 --- a/selfdrive/locationd/torqued.py +++ b/selfdrive/locationd/torqued.py @@ -22,15 +22,15 @@ FIT_POINTS_TOTAL_QLOG = 600 MIN_VEL = 15 # m/s FRICTION_FACTOR = 1.5 # ~85% of data coverage FACTOR_SANITY = 0.3 +FACTOR_SANITY_QLOG = 0.5 FRICTION_SANITY = 0.5 +FRICTION_SANITY_QLOG = 0.8 STEER_MIN_THRESHOLD = 0.02 MIN_FILTER_DECAY = 50 MAX_FILTER_DECAY = 250 LAT_ACC_THRESHOLD = 1 STEER_BUCKET_BOUNDS = [(-0.5, -0.3), (-0.3, -0.2), (-0.2, -0.1), (-0.1, 0), (0, 0.1), (0.1, 0.2), (0.2, 0.3), (0.3, 0.5)] MIN_BUCKET_POINTS = np.array([100, 300, 500, 500, 500, 500, 300, 100]) -MAX_RESETS = 5.0 -MAX_INVALID_THRESHOLD = 10 MIN_ENGAGE_BUFFER = 2 # secs VERSION = 1 # bump this to invalidate old parameter caches @@ -100,10 +100,15 @@ class TorqueEstimator: self.min_bucket_points = MIN_BUCKET_POINTS / 10 self.min_points_total = MIN_POINTS_TOTAL_QLOG self.fit_points = FIT_POINTS_TOTAL_QLOG + self.factor_sanity = FACTOR_SANITY_QLOG + self.friction_sanity = FRICTION_SANITY_QLOG + else: self.min_bucket_points = MIN_BUCKET_POINTS self.min_points_total = MIN_POINTS_TOTAL self.fit_points = FIT_POINTS_TOTAL + self.factor_sanity = FACTOR_SANITY + self.friction_sanity = FRICTION_SANITY self.offline_friction = 0.0 self.offline_latAccelFactor = 0.0 @@ -123,10 +128,10 @@ class TorqueEstimator: 'points': [] } self.decay = MIN_FILTER_DECAY - self.min_lataccel_factor = (1.0 - FACTOR_SANITY) * self.offline_latAccelFactor - self.max_lataccel_factor = (1.0 + FACTOR_SANITY) * self.offline_latAccelFactor - self.min_friction = (1.0 - FRICTION_SANITY) * self.offline_friction - self.max_friction = (1.0 + FRICTION_SANITY) * self.offline_friction + self.min_lataccel_factor = (1.0 - self.factor_sanity) * self.offline_latAccelFactor + self.max_lataccel_factor = (1.0 + self.factor_sanity) * self.offline_latAccelFactor + self.min_friction = (1.0 - self.friction_sanity) * self.offline_friction + self.max_friction = (1.0 + self.friction_sanity) * self.offline_friction # try to restore cached params params = Params() @@ -165,7 +170,6 @@ class TorqueEstimator: def reset(self): self.resets += 1.0 - self.invalid_values_tracker = 0.0 self.decay = MIN_FILTER_DECAY self.raw_points = defaultdict(lambda: deque(maxlen=self.hist_len)) self.filtered_points = PointBuckets(x_bounds=STEER_BUCKET_BOUNDS, min_points=self.min_bucket_points, min_points_total=self.min_points_total) @@ -190,12 +194,6 @@ class TorqueEstimator: self.filtered_params[param].update(value) self.filtered_params[param].update_alpha(self.decay) - def is_sane(self, latAccelFactor, latAccelOffset, friction): - if any([val is None or np.isnan(val) for val in [latAccelFactor, latAccelOffset, friction]]): - return False - return (self.max_friction >= friction >= self.min_friction) and\ - (self.max_lataccel_factor >= latAccelFactor >= self.min_lataccel_factor) - def handle_log(self, t, which, msg): if which == "carControl": self.raw_points["carControl_t"].append(t + self.lag) @@ -225,23 +223,20 @@ class TorqueEstimator: liveTorqueParameters.useParams = self.use_params if self.filtered_points.is_valid(): - latAccelFactor, latAccelOffset, friction_coeff = self.estimate_params() + latAccelFactor, latAccelOffset, frictionCoeff = self.estimate_params() liveTorqueParameters.latAccelFactorRaw = float(latAccelFactor) liveTorqueParameters.latAccelOffsetRaw = float(latAccelOffset) - liveTorqueParameters.frictionCoefficientRaw = float(friction_coeff) + liveTorqueParameters.frictionCoefficientRaw = float(frictionCoeff) - if self.is_sane(latAccelFactor, latAccelOffset, friction_coeff): - liveTorqueParameters.liveValid = True - self.update_params({'latAccelFactor': latAccelFactor, 'latAccelOffset': latAccelOffset, 'frictionCoefficient': friction_coeff}) - self.invalid_values_tracker = max(0.0, self.invalid_values_tracker - 0.5) - else: - cloudlog.exception("Live torque parameters are outside acceptable bounds.") + if any([val is None or np.isnan(val) for val in [latAccelFactor, latAccelOffset, frictionCoeff]]): + cloudlog.exception("Live torque parameters are invalid.") liveTorqueParameters.liveValid = False - self.invalid_values_tracker += 1.0 - # Reset when ~10 invalid over 5 secs - if self.invalid_values_tracker > MAX_INVALID_THRESHOLD: - # Do not reset the filter as it may cause a drastic jump, just reset points - self.reset() + self.reset() + else: + liveTorqueParameters.liveValid = True + latAccelFactor = np.clip(latAccelFactor, self.min_lataccel_factor, self.max_lataccel_factor) + frictionCoeff = np.clip(frictionCoeff, self.min_friction, self.max_friction) + self.update_params({'latAccelFactor': latAccelFactor, 'latAccelOffset': latAccelOffset, 'frictionCoefficient': frictionCoeff}) else: liveTorqueParameters.liveValid = False diff --git a/selfdrive/locationd/ublox_msg.cc b/selfdrive/locationd/ublox_msg.cc index 6431e9d48b..b746989466 100644 --- a/selfdrive/locationd/ublox_msg.cc +++ b/selfdrive/locationd/ublox_msg.cc @@ -65,6 +65,21 @@ inline bool UbloxMsgParser::valid_so_far() { return true; } +inline uint16_t UbloxMsgParser::get_glonass_year(uint8_t N4, uint16_t Nt) { + // convert time to year (conversion from A3.1.3) + int J = 0; + if (1 <= Nt && Nt <= 366) { + J = 1; + } else if (367 <= Nt && Nt <= 731) { + J = 2; + } else if (732 <= Nt && Nt <= 1096) { + J = 3; + } else if (1097 <= Nt && Nt <= 1461) { + J = 4; + } + uint16_t year = 1996 + 4*(N4 -1) + (J - 1); + return year; +} bool UbloxMsgParser::add_data(const uint8_t *incoming_data, uint32_t incoming_data_len, size_t &bytes_consumed) { int needed = needed_bytes(); @@ -103,9 +118,9 @@ std::pair> UbloxMsgParser::gen_msg() { switch (ubx_message.msg_type()) { case 0x0107: return {"gpsLocationExternal", gen_nav_pvt(static_cast(body))}; - case 0x0213: + case 0x0213: // UBX-RXM-SFRB (Broadcast Navigation Data Subframe) return {"ubloxGnss", gen_rxm_sfrbx(static_cast(body))}; - case 0x0215: + case 0x0215: // UBX-RXM-RAW (Multi-GNSS Raw Measurement Data) return {"ubloxGnss", gen_rxm_rawx(static_cast(body))}; case 0x0a09: return {"ubloxGnss", gen_mon_hw(static_cast(body))}; @@ -147,116 +162,256 @@ kj::Array UbloxMsgParser::gen_nav_pvt(ubx_t::nav_pvt_t *msg) { return capnp::messageToFlatArray(msg_builder); } - -kj::Array UbloxMsgParser::gen_rxm_sfrbx(ubx_t::rxm_sfrbx_t *msg) { +kj::Array UbloxMsgParser::parse_gps_ephemeris(ubx_t::rxm_sfrbx_t *msg) { + // GPS subframes are packed into 10x 4 bytes, each containing 3 actual bytes + // We will first need to separate the data from the padding and parity auto body = *msg->body(); + assert(body.size() == 10); + + std::string subframe_data; + subframe_data.reserve(30); + for (uint32_t word : body) { + word = word >> 6; // TODO: Verify parity + subframe_data.push_back(word >> 16); + subframe_data.push_back(word >> 8); + subframe_data.push_back(word >> 0); + } - if (msg->gnss_id() == ubx_t::gnss_type_t::GNSS_TYPE_GPS) { - // GPS subframes are packed into 10x 4 bytes, each containing 3 actual bytes - // We will first need to separate the data from the padding and parity - assert(body.size() == 10); + // Collect subframes in map and parse when we have all the parts + { + kaitai::kstream stream(subframe_data); + gps_t subframe(&stream); - std::string subframe_data; - subframe_data.reserve(30); - for (uint32_t word : body) { - word = word >> 6; // TODO: Verify parity - subframe_data.push_back(word >> 16); - subframe_data.push_back(word >> 8); - subframe_data.push_back(word >> 0); + int subframe_id = subframe.how()->subframe_id(); + if (subframe_id > 3) { + // dont parse almanac subframes + return kj::Array(); + } + gps_subframes[msg->sv_id()][subframe_id] = subframe_data; + } + + // publish if subframes 1-3 have been collected + if (gps_subframes[msg->sv_id()].size() == 3) { + MessageBuilder msg_builder; + auto eph = msg_builder.initEvent().initUbloxGnss().initEphemeris(); + eph.setSvId(msg->sv_id()); + + int iode_s2 = 0; + int iode_s3 = 0; + int iodc_lsb = 0; + + // Subframe 1 + { + kaitai::kstream stream(gps_subframes[msg->sv_id()][1]); + gps_t subframe(&stream); + gps_t::subframe_1_t* subframe_1 = static_cast(subframe.body()); + + eph.setGpsWeek(subframe_1->week_no()); + eph.setTgd(subframe_1->t_gd() * pow(2, -31)); + eph.setToc(subframe_1->t_oc() * pow(2, 4)); + eph.setAf2(subframe_1->af_2() * pow(2, -55)); + eph.setAf1(subframe_1->af_1() * pow(2, -43)); + eph.setAf0(subframe_1->af_0() * pow(2, -31)); + eph.setSvHealth(subframe_1->sv_health()); + eph.setTowCount(subframe.how()->tow_count()); + iodc_lsb = subframe_1->iodc_lsb(); } - // Collect subframes in map and parse when we have all the parts + // Subframe 2 { - kaitai::kstream stream(subframe_data); + kaitai::kstream stream(gps_subframes[msg->sv_id()][2]); gps_t subframe(&stream); - int subframe_id = subframe.how()->subframe_id(); + gps_t::subframe_2_t* subframe_2 = static_cast(subframe.body()); + + eph.setCrs(subframe_2->c_rs() * pow(2, -5)); + eph.setDeltaN(subframe_2->delta_n() * pow(2, -43) * gpsPi); + eph.setM0(subframe_2->m_0() * pow(2, -31) * gpsPi); + eph.setCuc(subframe_2->c_uc() * pow(2, -29)); + eph.setEcc(subframe_2->e() * pow(2, -33)); + eph.setCus(subframe_2->c_us() * pow(2, -29)); + eph.setA(pow(subframe_2->sqrt_a() * pow(2, -19), 2.0)); + eph.setToe(subframe_2->t_oe() * pow(2, 4)); + iode_s2 = subframe_2->iode(); + } - if (subframe_id == 1) gps_subframes[msg->sv_id()].clear(); - gps_subframes[msg->sv_id()][subframe_id] = subframe_data; + // Subframe 3 + { + kaitai::kstream stream(gps_subframes[msg->sv_id()][3]); + gps_t subframe(&stream); + gps_t::subframe_3_t* subframe_3 = static_cast(subframe.body()); + + eph.setCic(subframe_3->c_ic() * pow(2, -29)); + eph.setOmega0(subframe_3->omega_0() * pow(2, -31) * gpsPi); + eph.setCis(subframe_3->c_is() * pow(2, -29)); + eph.setI0(subframe_3->i_0() * pow(2, -31) * gpsPi); + eph.setCrc(subframe_3->c_rc() * pow(2, -5)); + eph.setOmega(subframe_3->omega() * pow(2, -31) * gpsPi); + eph.setOmegaDot(subframe_3->omega_dot() * pow(2, -43) * gpsPi); + eph.setIode(subframe_3->iode()); + eph.setIDot(subframe_3->idot() * pow(2, -43) * gpsPi); + iode_s3 = subframe_3->iode(); } - if (gps_subframes[msg->sv_id()].size() == 5) { - MessageBuilder msg_builder; - auto eph = msg_builder.initEvent().initUbloxGnss().initEphemeris(); - eph.setSvId(msg->sv_id()); - - // Subframe 1 - { - kaitai::kstream stream(gps_subframes[msg->sv_id()][1]); - gps_t subframe(&stream); - gps_t::subframe_1_t* subframe_1 = static_cast(subframe.body()); - - eph.setGpsWeek(subframe_1->week_no()); - eph.setTgd(subframe_1->t_gd() * pow(2, -31)); - eph.setToc(subframe_1->t_oc() * pow(2, 4)); - eph.setAf2(subframe_1->af_2() * pow(2, -55)); - eph.setAf1(subframe_1->af_1() * pow(2, -43)); - eph.setAf0(subframe_1->af_0() * pow(2, -31)); - eph.setSvHealth(subframe_1->sv_health()); - } - - // Subframe 2 - { - kaitai::kstream stream(gps_subframes[msg->sv_id()][2]); - gps_t subframe(&stream); - gps_t::subframe_2_t* subframe_2 = static_cast(subframe.body()); - - eph.setCrs(subframe_2->c_rs() * pow(2, -5)); - eph.setDeltaN(subframe_2->delta_n() * pow(2, -43) * gpsPi); - eph.setM0(subframe_2->m_0() * pow(2, -31) * gpsPi); - eph.setCuc(subframe_2->c_uc() * pow(2, -29)); - eph.setEcc(subframe_2->e() * pow(2, -33)); - eph.setCus(subframe_2->c_us() * pow(2, -29)); - eph.setA(pow(subframe_2->sqrt_a() * pow(2, -19), 2.0)); - eph.setToe(subframe_2->t_oe() * pow(2, 4)); - } - - // Subframe 3 - { - kaitai::kstream stream(gps_subframes[msg->sv_id()][3]); - gps_t subframe(&stream); - gps_t::subframe_3_t* subframe_3 = static_cast(subframe.body()); - - eph.setCic(subframe_3->c_ic() * pow(2, -29)); - eph.setOmega0(subframe_3->omega_0() * pow(2, -31) * gpsPi); - eph.setCis(subframe_3->c_is() * pow(2, -29)); - eph.setI0(subframe_3->i_0() * pow(2, -31) * gpsPi); - eph.setCrc(subframe_3->c_rc() * pow(2, -5)); - eph.setOmega(subframe_3->omega() * pow(2, -31) * gpsPi); - eph.setOmegaDot(subframe_3->omega_dot() * pow(2, -43) * gpsPi); - eph.setIode(subframe_3->iode()); - eph.setIDot(subframe_3->idot() * pow(2, -43) * gpsPi); - } - - // Subframe 4 - { - kaitai::kstream stream(gps_subframes[msg->sv_id()][4]); - gps_t subframe(&stream); - gps_t::subframe_4_t* subframe_4 = static_cast(subframe.body()); - - // This is page 18, why is the page id 56? - if (subframe_4->data_id() == 1 && subframe_4->page_id() == 56) { - auto iono = static_cast(subframe_4->body()); - double a0 = iono->a0() * pow(2, -30); - double a1 = iono->a1() * pow(2, -27); - double a2 = iono->a2() * pow(2, -24); - double a3 = iono->a3() * pow(2, -24); - eph.setIonoAlpha({a0, a1, a2, a3}); - - double b0 = iono->b0() * pow(2, 11); - double b1 = iono->b1() * pow(2, 14); - double b2 = iono->b2() * pow(2, 16); - double b3 = iono->b3() * pow(2, 16); - eph.setIonoBeta({b0, b1, b2, b3}); - } - } - - return capnp::messageToFlatArray(msg_builder); + gps_subframes[msg->sv_id()].clear(); + if (iodc_lsb != iode_s2 || iodc_lsb != iode_s3) { + // data set cutover, reject ephemeris + return kj::Array(); } + return capnp::messageToFlatArray(msg_builder); } return kj::Array(); } +kj::Array UbloxMsgParser::parse_glonass_ephemeris(ubx_t::rxm_sfrbx_t *msg) { + if (msg->sv_id() == 255) { + // data can be decoded before identifying the SV number, in this case 255 + // is returned, which means "unknown" (ublox p32) + return kj::Array(); + } + + auto body = *msg->body(); + assert(body.size() == 4); + { + std::string string_data; + string_data.reserve(16); + for (uint32_t word : body) { + for (int i = 3; i >= 0; i--) + string_data.push_back(word >> 8*i); + } + + kaitai::kstream stream(string_data); + glonass_t gl_string(&stream); + + int string_number = gl_string.string_number(); + if (string_number > 5 || gl_string.idle_chip()) { + // dont parse non immediate data, idle_chip == 0 + return kj::Array(); + } + + // immediate data is the same within one superframe + if (glonass_superframes[msg->sv_id()] != gl_string.superframe_number()) { + glonass_strings[msg->sv_id()].clear(); + glonass_superframes[msg->sv_id()] = gl_string.superframe_number(); + } + glonass_strings[msg->sv_id()][string_number] = string_data; + } + + // publish if strings 1-5 have been collected + if (glonass_strings[msg->sv_id()].size() != 5) { + return kj::Array(); + } + + MessageBuilder msg_builder; + auto eph = msg_builder.initEvent().initUbloxGnss().initGlonassEphemeris(); + eph.setSvId(msg->sv_id()); + uint16_t current_day = 0; + + // string number 1 + { + kaitai::kstream stream(glonass_strings[msg->sv_id()][1]); + glonass_t gl_stream(&stream); + glonass_t::string_1_t* data = static_cast(gl_stream.data()); + + eph.setP1(data->p1()); + eph.setTk(data->t_k()); + eph.setXVel(data->x_vel() * pow(2, -20)); + eph.setXAccel(data->x_accel() * pow(2, -30)); + eph.setX(data->x() * pow(2, -11)); + } + + // string number 2 + { + kaitai::kstream stream(glonass_strings[msg->sv_id()][2]); + glonass_t gl_stream(&stream); + glonass_t::string_2_t* data = static_cast(gl_stream.data()); + + eph.setSvHealth(data->b_n()>>2); // MSB indicates health + eph.setP2(data->p2()); + eph.setTb(data->t_b()); + eph.setYVel(data->y_vel() * pow(2, -20)); + eph.setYAccel(data->y_accel() * pow(2, -30)); + eph.setY(data->y() * pow(2, -11)); + } + + // string number 3 + { + kaitai::kstream stream(glonass_strings[msg->sv_id()][3]); + glonass_t gl_stream(&stream); + glonass_t::string_3_t* data = static_cast(gl_stream.data()); + + eph.setP3(data->p3()); + eph.setGammaN(data->gamma_n() * pow(2, -40)); + eph.setSvHealth(eph.getSvHealth() | data->l_n()); + eph.setZVel(data->z_vel() * pow(2, -20)); + eph.setZAccel(data->z_accel() * pow(2, -30)); + eph.setZ(data->z() * pow(2, -11)); + } + + // string number 4 + { + kaitai::kstream stream(glonass_strings[msg->sv_id()][4]); + glonass_t gl_stream(&stream); + glonass_t::string_4_t* data = static_cast(gl_stream.data()); + + current_day = data->n_t(); + eph.setTauN(data->tau_n() * pow(2, -30)); + eph.setDeltaTauN(data->delta_tau_n() * pow(2, -30)); + eph.setAge(data->e_n()); + eph.setP4(data->p4()); + eph.setSvURA(glonass_URA_lookup.at(data->f_t())); + if (msg->sv_id() != data->n()) { + LOGE("SV_ID != SLOT_NUMBER: %d %d", msg->sv_id(), data->n()) + } + eph.setSvType(data->m()); + } + + // string number 5 + { + kaitai::kstream stream(glonass_strings[msg->sv_id()][5]); + glonass_t gl_stream(&stream); + glonass_t::string_5_t* data = static_cast(gl_stream.data()); + + // string5 parsing is only needed to get the year, this can be removed and + // the year can be fetched later in laika (note rollovers and leap year) + uint8_t n_4 = data->n_4(); + uint16_t year = get_glonass_year(n_4, current_day); + if (current_day > 1461) { + // impossible day within last 4 year, reject ephemeris + // TODO: check if this can be detected via hamming code + LOGE("INVALID DATA: current day out of range: %d, %d", current_day, n_4); + glonass_strings[msg->sv_id()].clear(); + return kj::Array(); + } + + uint16_t last_leap_year = 1996 + 4*(n_4-1); + uint16_t days_till_this_year = (year - last_leap_year)*365; + if (days_till_this_year != 0) { + days_till_this_year++; + } + + eph.setYear(year); + eph.setDayInYear(current_day - days_till_this_year); + eph.setHour((eph.getTk()>>7) & 0x1F); + eph.setMinute((eph.getTk()>>1) & 0x3F); + eph.setSecond((eph.getTk() & 0x1) * 30); + } + + glonass_strings[msg->sv_id()].clear(); + return capnp::messageToFlatArray(msg_builder); +} + + +kj::Array UbloxMsgParser::gen_rxm_sfrbx(ubx_t::rxm_sfrbx_t *msg) { + switch (msg->gnss_id()) { + case ubx_t::gnss_type_t::GNSS_TYPE_GPS: + return parse_gps_ephemeris(msg); + case ubx_t::gnss_type_t::GNSS_TYPE_GLONASS: + return parse_glonass_ephemeris(msg); + default: + return kj::Array(); + } +} + kj::Array UbloxMsgParser::gen_rxm_rawx(ubx_t::rxm_rawx_t *msg) { MessageBuilder msg_builder; auto mr = msg_builder.initEvent().initUbloxGnss().initMeasurementReport(); diff --git a/selfdrive/locationd/ublox_msg.h b/selfdrive/locationd/ublox_msg.h index 542e72816b..6988f20b74 100644 --- a/selfdrive/locationd/ublox_msg.h +++ b/selfdrive/locationd/ublox_msg.h @@ -10,6 +10,7 @@ #include "cereal/messaging/messaging.h" #include "common/util.h" #include "selfdrive/locationd/generated/gps.h" +#include "selfdrive/locationd/generated/glonass.h" #include "selfdrive/locationd/generated/ubx.h" using namespace std::string_literals; @@ -101,11 +102,22 @@ class UbloxMsgParser { inline bool valid_cheksum(); inline bool valid(); inline bool valid_so_far(); + inline uint16_t get_glonass_year(uint8_t N4, uint16_t Nt); + + kj::Array parse_gps_ephemeris(ubx_t::rxm_sfrbx_t *msg); + kj::Array parse_glonass_ephemeris(ubx_t::rxm_sfrbx_t *msg); std::unordered_map> gps_subframes; size_t bytes_in_parse_buf = 0; uint8_t msg_parse_buf[ublox::UBLOX_HEADER_SIZE + ublox::UBLOX_MAX_MSG_SIZE]; -}; + // user range accuracy in meters + const std::unordered_map glonass_URA_lookup = + {{ 0, 1}, { 1, 2}, { 2, 2.5}, { 3, 4}, { 4, 5}, {5, 7}, + { 6, 10}, { 7, 12}, { 8, 14}, { 9, 16}, {10, 32}, + {11, 64}, {12, 128}, {13, 256}, {14, 512}, {15, 1024}}; + std::unordered_map> glonass_strings; + std::unordered_map glonass_superframes; +}; diff --git a/selfdrive/loggerd/SConscript b/selfdrive/loggerd/SConscript index 92706c53ec..3b961bce6e 100644 --- a/selfdrive/loggerd/SConscript +++ b/selfdrive/loggerd/SConscript @@ -13,6 +13,8 @@ if arch == "Darwin": # fix OpenCL del libs[libs.index('OpenCL')] env['FRAMEWORKS'] = ['OpenCL'] + # exclude v4l + del src[src.index('encoder/v4l_encoder.cc')] logger_lib = env.Library('logger', src) libs.insert(0, logger_lib) diff --git a/selfdrive/loggerd/loggerd.cc b/selfdrive/loggerd/loggerd.cc index 9beb3c3bf1..e09cdfaa9e 100644 --- a/selfdrive/loggerd/loggerd.cc +++ b/selfdrive/loggerd/loggerd.cc @@ -24,15 +24,25 @@ void logger_rotate(LoggerdState *s) { } void rotate_if_needed(LoggerdState *s) { - if (s->ready_to_rotate == s->max_waiting) { - logger_rotate(s); - } + // all encoders ready, trigger rotation + bool all_ready = s->ready_to_rotate == s->max_waiting; + // fallback logic to prevent extremely long segments in the case of camera, encoder, etc. malfunctions + bool timed_out = false; double tms = millis_since_boot(); - if ((tms - s->last_rotate_tms) > SEGMENT_LENGTH * 1000 && - (tms - s->last_camera_seen_tms) > NO_CAMERA_PATIENCE && - !LOGGERD_TEST) { - LOGW("no camera packet seen. auto rotating"); + double seg_length_secs = (tms - s->last_rotate_tms) / 1000.; + if ((seg_length_secs > SEGMENT_LENGTH) && !LOGGERD_TEST) { + // TODO: might be nice to put these reasons in the sentinel + if ((tms - s->last_camera_seen_tms) > NO_CAMERA_PATIENCE) { + timed_out = true; + LOGE("no camera packets seen. auto rotating"); + } else if (seg_length_secs > SEGMENT_LENGTH*1.2) { + timed_out = true; + LOGE("segment too long. auto rotating"); + } + } + + if (all_ready || timed_out) { logger_rotate(s); } } diff --git a/selfdrive/manager/build.py b/selfdrive/manager/build.py index c8a7d41539..5b69e3dca7 100755 --- a/selfdrive/manager/build.py +++ b/selfdrive/manager/build.py @@ -15,7 +15,7 @@ from system.version import is_dirty MAX_CACHE_SIZE = 4e9 if "CI" in os.environ else 2e9 CACHE_DIR = Path("/data/scons_cache" if AGNOS else "/tmp/scons_cache") -TOTAL_SCONS_NODES = 2395 +TOTAL_SCONS_NODES = 2460 MAX_BUILD_PROGRESS = 100 PREBUILT = os.path.exists(os.path.join(BASEDIR, 'prebuilt')) diff --git a/selfdrive/manager/manager.py b/selfdrive/manager/manager.py index 369c529626..865966d6c5 100755 --- a/selfdrive/manager/manager.py +++ b/selfdrive/manager/manager.py @@ -20,7 +20,7 @@ from selfdrive.manager.process_config import managed_processes from selfdrive.athena.registration import register, UNREGISTERED_DONGLE_ID from system.swaglog import cloudlog, add_file_handler from system.version import is_dirty, get_commit, get_version, get_origin, get_short_branch, \ - terms_version, training_version, is_tested_branch + terms_version, training_version, is_tested_branch, is_release_branch @@ -76,6 +76,7 @@ def manager_init() -> None: params.put("GitBranch", get_short_branch(default="")) params.put("GitRemote", get_origin(default="")) params.put_bool("IsTestedBranch", is_tested_branch()) + params.put_bool("IsReleaseBranch", is_release_branch()) # set dongle id reg_res = register(show_spinner=True) diff --git a/selfdrive/manager/process.py b/selfdrive/manager/process.py index dabfbe4ee0..ce18073d9b 100644 --- a/selfdrive/manager/process.py +++ b/selfdrive/manager/process.py @@ -290,10 +290,11 @@ class DaemonProcess(ManagerProcess): def ensure_running(procs: ValuesView[ManagerProcess], started: bool, params=None, CP: car.CarParams=None, - not_run: Optional[List[str]]=None) -> None: + not_run: Optional[List[str]]=None) -> List[ManagerProcess]: if not_run is None: not_run = [] + running = [] for p in procs: # Conditions that make a process run run = any(( @@ -311,7 +312,10 @@ def ensure_running(procs: ValuesView[ManagerProcess], started: bool, params=None if run: p.start() + running.append(p) else: p.stop(block=False) p.check_watchdog(started) + + return running diff --git a/selfdrive/manager/process_config.py b/selfdrive/manager/process_config.py index c03e995497..8fc4d94e55 100644 --- a/selfdrive/manager/process_config.py +++ b/selfdrive/manager/process_config.py @@ -17,6 +17,11 @@ def logging(started, params, CP: car.CarParams) -> bool: run = (not CP.notCar) or not params.get_bool("DisableLogging") return started and run +def ublox(started, params, CP: car.CarParams) -> bool: + use_ublox = os.path.exists('/dev/ttyHS0') and not os.path.exists('/persist/comma/use-quectel-gps') + params.put_bool("UbloxAvailable", use_ublox) + return started and use_ublox + procs = [ # due to qualcomm kernel bugs SIGKILLing camerad sometimes causes page table corruption NativeProcess("camerad", "system/camerad", ["./camerad"], unkillable=True, callback=driverview), @@ -35,7 +40,6 @@ procs = [ NativeProcess("mapsd", "selfdrive/navd", ["./map_renderer"], enabled=False), NativeProcess("navmodeld", "selfdrive/modeld", ["./navmodeld"], enabled=False), NativeProcess("sensord", "selfdrive/sensord", ["./sensord"], enabled=not PC), - NativeProcess("ubloxd", "selfdrive/locationd", ["./ubloxd"], enabled=(not PC or WEBCAM)), NativeProcess("ui", "selfdrive/ui", ["./ui"], offroad=True, watchdog_max_dt=(5 if not PC else None)), NativeProcess("soundd", "selfdrive/ui/soundd", ["./soundd"], offroad=True), NativeProcess("locationd", "selfdrive/locationd", ["./locationd"]), @@ -50,7 +54,8 @@ procs = [ PythonProcess("navd", "selfdrive.navd.navd"), PythonProcess("pandad", "selfdrive.boardd.pandad", offroad=True), PythonProcess("paramsd", "selfdrive.locationd.paramsd"), - PythonProcess("pigeond", "selfdrive.sensord.pigeond", enabled=TICI), + NativeProcess("ubloxd", "selfdrive/locationd", ["./ubloxd"], enabled=TICI, onroad=False, callback=ublox), + PythonProcess("pigeond", "selfdrive.sensord.pigeond", enabled=TICI, onroad=False, callback=ublox), PythonProcess("plannerd", "selfdrive.controls.plannerd"), PythonProcess("radard", "selfdrive.controls.radard"), PythonProcess("thermald", "selfdrive.thermald.thermald", offroad=True), diff --git a/selfdrive/manager/test/test_manager.py b/selfdrive/manager/test/test_manager.py index 6d4df0423a..889ab3d279 100755 --- a/selfdrive/manager/test/test_manager.py +++ b/selfdrive/manager/test/test_manager.py @@ -4,17 +4,17 @@ import signal import time import unittest +from cereal import car from common.params import Params import selfdrive.manager.manager as manager -from selfdrive.manager.process import DaemonProcess +from selfdrive.manager.process import ensure_running from selfdrive.manager.process_config import managed_processes from system.hardware import HARDWARE os.environ['FAKEUPLOAD'] = "1" MAX_STARTUP_TIME = 3 -ALL_PROCESSES = [p.name for p in managed_processes.values() if (type(p) is not DaemonProcess) and p.enabled and (p.name not in ['pandad', ])] - +BLACKLIST_PROCS = ['manage_athenad', 'pandad', 'pigeond'] class TestManager(unittest.TestCase): def setUp(self): @@ -47,24 +47,25 @@ class TestManager(unittest.TestCase): HARDWARE.set_power_save(False) manager.manager_init() manager.manager_prepare() - for p in ALL_PROCESSES: - managed_processes[p].start() + + CP = car.CarParams.new_message() + procs = ensure_running(managed_processes.values(), True, Params(), CP, not_run=BLACKLIST_PROCS) time.sleep(10) - for p in reversed(ALL_PROCESSES): - with self.subTest(proc=p): - state = managed_processes[p].get_process_state_msg() - self.assertTrue(state.running, f"{p} not running") - exit_code = managed_processes[p].stop(retry=False) + for p in procs: + with self.subTest(proc=p.name): + state = p.get_process_state_msg() + self.assertTrue(state.running, f"{p.name} not running") + exit_code = p.stop(retry=False) - self.assertTrue(exit_code is not None, f"{p} failed to exit") + self.assertTrue(exit_code is not None, f"{p.name} failed to exit") # TODO: interrupted blocking read exits with 1 in cereal. use a more unique return code exit_codes = [0, 1] - if managed_processes[p].sigkill: + if p.sigkill: exit_codes = [-signal.SIGKILL] - self.assertIn(exit_code, exit_codes, f"{p} died with {exit_code}") + self.assertIn(exit_code, exit_codes, f"{p.name} died with {exit_code}") if __name__ == "__main__": diff --git a/selfdrive/modeld/SConscript b/selfdrive/modeld/SConscript index 82338e456b..7bbc1b3477 100644 --- a/selfdrive/modeld/SConscript +++ b/selfdrive/modeld/SConscript @@ -70,30 +70,14 @@ lenv.Program('_dmonitoringmodeld', [ if use_thneed and arch == "larch64" or GetOption('pc_thneed'): fn = File("models/supercombo").abspath - if GetOption('pc_thneed'): - cmd = f"cd {Dir('#').abspath}/tinygrad_repo && GPU=1 NATIVE_EXPLOG=1 OPTWG=1 UNSAFE_FLOAT4=1 DEBUGCL=1 python3 openpilot/compile.py {fn}.onnx {fn}.thneed" - else: - cmd = f"cd {Dir('#').abspath}/tinygrad_repo && FLOAT16=1 MATMUL=1 PYOPENCL_NO_CACHE=1 NATIVE_EXPLOG=1 OPTWG=1 UNSAFE_FLOAT4=1 DEBUGCL=1 python3 openpilot/compile.py {fn}.onnx {fn}.thneed" - - # is there a better way then listing all of tinygrad? - lenv.Command(fn + ".thneed", [fn + ".onnx", - "#tinygrad_repo/openpilot/compile.py", - "#tinygrad_repo/accel/opencl/conv.cl", - "#tinygrad_repo/accel/opencl/matmul.cl", - "#tinygrad_repo/accel/opencl/ops_opencl.py", - "#tinygrad_repo/accel/opencl/preprocessing.py", - "#tinygrad_repo/extra/onnx.py", - "#tinygrad_repo/extra/thneed.py", - "#tinygrad_repo/extra/utils.py", - "#tinygrad_repo/tinygrad/llops/ops_gpu.py", - "#tinygrad_repo/tinygrad/llops/ops_opencl.py", - "#tinygrad_repo/tinygrad/helpers.py", - "#tinygrad_repo/tinygrad/mlops.py", - "#tinygrad_repo/tinygrad/ops.py", - "#tinygrad_repo/tinygrad/shapetracker.py", - "#tinygrad_repo/tinygrad/tensor.py", - "#tinygrad_repo/tinygrad/nn/__init__.py" - ], cmd) + tinygrad_opts = ["NATIVE_EXPLOG=1", "VALIDHACKS=1", "OPTWG=1", "IMAGE=2", "GPU=1", "CLCACHE=0"] + if not GetOption('pc_thneed'): + # use FLOAT16 on device for speed + don't cache the CL kernels for space + tinygrad_opts += ["FLOAT16=1", "PYOPENCL_NO_CACHE=1"] + cmd = f"cd {Dir('#').abspath}/tinygrad_repo && " + ' '.join(tinygrad_opts) + f" python3 openpilot/compile.py {fn}.onnx {fn}.thneed" + + tinygrad_files = sum([lenv.Glob("#"+x) for x in open(File("#release/files_common").abspath).read().split("\n") if x.startswith("tinygrad_repo/")], []) + lenv.Command(fn + ".thneed", [fn + ".onnx"] + tinygrad_files, cmd) llenv = lenv.Clone() if GetOption('pc_thneed'): @@ -116,4 +100,4 @@ llenv.Program('_modeld', [ lenv.Program('_navmodeld', [ "navmodeld.cc", "models/nav.cc", - ]+common_model, LIBS=libs + transformations) \ No newline at end of file + ]+common_model, LIBS=libs + transformations) diff --git a/selfdrive/modeld/models/supercombo.onnx b/selfdrive/modeld/models/supercombo.onnx index 8805b3dce8..a483fa4db4 100644 --- a/selfdrive/modeld/models/supercombo.onnx +++ b/selfdrive/modeld/models/supercombo.onnx @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:db746e3753de84367595fedd089c2bd41b06bd401ea28e085663533d0e63d74b -size 45962473 +oid sha256:736ddc08497d7596bae4d9515a8efb996676be80e67a6d34d632bb8af2ed3fa9 +size 45962515 diff --git a/selfdrive/navd/navd.py b/selfdrive/navd/navd.py index 81e3bdb3df..7af911ab2a 100755 --- a/selfdrive/navd/navd.py +++ b/selfdrive/navd/navd.py @@ -226,7 +226,11 @@ class RouteEngine: remaining = 1.0 - along_geometry / max(step['distance'], 1) total_distance = step['distance'] * remaining total_time = step['duration'] * remaining - total_time_typical = step['duration_typical'] * remaining + + if step['duration_typical'] is None: + total_time_typical = total_time + else: + total_time_typical = step['duration_typical'] * remaining # Add up totals for future steps for i in range(self.step_idx + 1, len(self.route)): diff --git a/selfdrive/sensord/pigeond.py b/selfdrive/sensord/pigeond.py index f56af1c705..9d0a62bd3b 100755 --- a/selfdrive/sensord/pigeond.py +++ b/selfdrive/sensord/pigeond.py @@ -123,7 +123,7 @@ class TTYPigeon(): init_baudrate(self) # clear configuration - self.send_with_ack(b"\xb5\x62\x06\x09\x0d\x00\x00\x00\x1f\x1f\x00\x00\x00\x00\x00\x00\x00\x00\x17\x71\x5b") + self.send_with_ack(b"\xb5\x62\x06\x09\x0d\x00\x1f\x1f\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x17\x71\xd7") # clear flash memory (almanac backup) self.send_with_ack(b"\xB5\x62\x09\x14\x04\x00\x01\x00\x00\x00\x22\xf0") @@ -303,8 +303,7 @@ def main(): pigeon, pm = create_pigeon() init_baudrate(pigeon) - r = initialize_pigeon(pigeon) - Params().put_bool("UbloxAvailable", r) + initialize_pigeon(pigeon) # start receiving data run_receiving(pigeon, pm) diff --git a/selfdrive/sensord/rawgps/rawgpsd.py b/selfdrive/sensord/rawgps/rawgpsd.py index 1c65051665..3fa5e927a2 100755 --- a/selfdrive/sensord/rawgps/rawgpsd.py +++ b/selfdrive/sensord/rawgps/rawgpsd.py @@ -11,7 +11,9 @@ from struct import unpack_from, calcsize, pack from cereal import log import cereal.messaging as messaging +from common.gpio import gpio_init, gpio_set from laika.gps_time import GPSTime +from system.hardware.tici.pins import GPIO from system.swaglog import cloudlog from selfdrive.sensord.rawgps.modemdiag import ModemDiag, DIAG_LOG_F, setup_logs, send_recv from selfdrive.sensord.rawgps.structs import (dict_unpacker, position_report, relist, @@ -182,6 +184,7 @@ def main() -> NoReturn: def cleanup(sig, frame): cloudlog.warning(f"caught sig {sig}, disabling quectel gps") + gpio_set(GPIO.UBLOX_PWR_EN, False) teardown_quectel(diag) cloudlog.warning("quectel cleanup done") sys.exit(0) @@ -190,6 +193,8 @@ def main() -> NoReturn: setup_quectel(diag) cloudlog.warning("quectel setup done") + gpio_init(GPIO.UBLOX_PWR_EN, True) + gpio_set(GPIO.UBLOX_PWR_EN, True) pm = messaging.PubMaster(['qcomGnss', 'gpsLocation']) diff --git a/selfdrive/test/process_replay/model_replay_ref_commit b/selfdrive/test/process_replay/model_replay_ref_commit index 22e021dcaf..c64f522352 100644 --- a/selfdrive/test/process_replay/model_replay_ref_commit +++ b/selfdrive/test/process_replay/model_replay_ref_commit @@ -1 +1 @@ -db587bfef2317c5a3471632ac47381457e1be853 \ No newline at end of file +ba947edbb131a2a36ced7c490dfcf3280ad5b167 diff --git a/selfdrive/test/process_replay/process_replay.py b/selfdrive/test/process_replay/process_replay.py index 24ee87e471..b531cb3430 100755 --- a/selfdrive/test/process_replay/process_replay.py +++ b/selfdrive/test/process_replay/process_replay.py @@ -188,7 +188,7 @@ def get_car_params(msgs, fsm, can_sock, fingerprint): canmsgs = [msg for msg in msgs if msg.which() == 'can'] for m in canmsgs[:300]: can.send(m.as_builder().to_bytes()) - _, CP = get_car(can, sendcan) + _, CP = get_car(can, sendcan, Params().get_bool("ExperimentalLongitudinalEnabled")) Params().put("CarParams", CP.to_bytes()) @@ -204,7 +204,7 @@ def controlsd_rcv_callback(msg, CP, cfg, fsm): def radar_rcv_callback(msg, CP, cfg, fsm): if msg.which() != "can": return [], False - elif CP.radarOffCan: + elif CP.radarUnavailable: return ["radarState", "liveTracks"], True radar_msgs = {"honda": [0x445], "toyota": [0x19f, 0x22f], "gm": [0x474], @@ -331,7 +331,7 @@ CONFIGS = [ pub_sub={ "cameraOdometry": ["liveLocationKalman"], "accelerometer": [], "gyroscope": [], - "gnssMeasurements": [], "liveCalibration": [], "carState": [], + "gpsLocationExternal": [], "liveCalibration": [], "carState": [], }, ignore=["logMonoTime", "valid"], init_callback=get_car_params, @@ -448,6 +448,9 @@ def setup_env(simulation=False, CP=None, cfg=None, controlsState=None): if CP.openpilotLongitudinalControl: params.put_bool("ExperimentalLongitudinalEnabled", True) + # controlsd process configuration assume all routes are out of dashcam + params.put_bool("DashcamOverride", True) + def python_replay_process(cfg, lr, fingerprint=None): sub_sockets = [s for _, sub in cfg.pub_sub.items() for s in sub] @@ -554,7 +557,7 @@ def cpp_replay_process(cfg, lr, fingerprint=None): managed_processes[cfg.proc_name].start() try: - with Timeout(TIMEOUT): + with Timeout(TIMEOUT, error_msg=f"timed out testing process {repr(cfg.proc_name)}"): while not all(pm.all_readers_updated(s) for s in cfg.pub_sub.keys()): time.sleep(0) @@ -568,7 +571,7 @@ def cpp_replay_process(cfg, lr, fingerprint=None): resp_sockets = cfg.pub_sub[msg.which()] if cfg.should_recv_callback is None else cfg.should_recv_callback(msg) for s in resp_sockets: - response = messaging.recv_one(sockets[s]) + response = messaging.recv_one_retry(sockets[s]) if response is None: print(f"Warning, no response received {i}") diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index ea45fb4957..10665ae18c 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -c714ab010923f2bce732bd22e903ccc4454136fd \ No newline at end of file +69e52f02fd21844ff068c495b7fcb01ebc53bea5 \ No newline at end of file diff --git a/selfdrive/test/process_replay/test_processes.py b/selfdrive/test/process_replay/test_processes.py index c58909bf7f..3ff3d36ebc 100755 --- a/selfdrive/test/process_replay/test_processes.py +++ b/selfdrive/test/process_replay/test_processes.py @@ -29,6 +29,7 @@ source_segments = [ ("SUBARU", "341dccd5359e3c97|2022-09-12--10-35-33--3"), # SUBARU.OUTBACK ("GM", "0c58b6a25109da2b|2021-02-23--16-35-50--11"), # GM.VOLT ("GM2", "376bf99325883932|2022-10-27--13-41-22--1"), # GM.BOLT_EUV + ("FORD", "54827bf84c38b14f|2023-01-26--21-59-07--4"), # FORD.BRONCO_SPORT_MK1 ("NISSAN", "35336926920f3571|2021-02-12--18-38-48--46"), # NISSAN.XTRAIL ("VOLKSWAGEN", "de9592456ad7d144|2021-06-29--11-00-15--6"), # VOLKSWAGEN.GOLF ("MAZDA", "bd6a637565e91581|2021-10-30--15-14-53--4"), # MAZDA.CX9_2021 @@ -47,11 +48,12 @@ segments = [ ("TOYOTA3", "regen89026F6BD8D|2022-09-27--15-45-37--0"), ("HONDA", "regenC7D5645EB17|2022-09-27--15-47-29--0"), ("HONDA2", "regenCC2ECCE5742|2022-09-27--16-18-01--0"), - ("CHRYSLER", "regenC253C4DAC90|2022-09-27--15-51-03--0"), + ("CHRYSLER", "regenC253C4DAC90|2023-02-10--15-51-03--0"), ("RAM", "regen20490083AE7|2022-09-27--15-53-15--0"), ("SUBARU", "regen1E72BBDCED5|2022-09-27--15-55-31--0"), ("GM", "regen45B05A80EF6|2022-09-27--15-57-22--0"), ("GM2", "376bf99325883932|2022-10-27--13-41-22--1"), + ("FORD", "54827bf84c38b14f|2023-01-26--21-59-07--4"), ("NISSAN", "regenC19D899B46D|2022-09-27--15-59-13--0"), ("VOLKSWAGEN", "regenD8F7AC4BD0D|2022-09-27--16-41-45--0"), ("MAZDA", "regenFC3F9ECBB64|2022-09-27--16-03-09--0"), diff --git a/selfdrive/ui/SConscript b/selfdrive/ui/SConscript index 669c214746..0f28f5ccc3 100644 --- a/selfdrive/ui/SConscript +++ b/selfdrive/ui/SConscript @@ -1,4 +1,5 @@ import os +import json Import('qt_env', 'arch', 'common', 'messaging', 'visionipc', 'cereal', 'transformations') @@ -41,6 +42,7 @@ assets_src = "#selfdrive/assets/assets.qrc" qt_env.Command(assets, assets_src, f"rcc $SOURCES -o $TARGET") qt_env.Depends(assets, Glob('#selfdrive/assets/*', exclude=[assets, assets_src, "#selfdrive/assets/assets.o"])) asset_obj = qt_env.Object("assets", assets) +Export('asset_obj') # build soundd qt_env.Program("soundd/_soundd", ["soundd/main.cc", "soundd/sound.cc"], LIBS=qt_libs) @@ -66,11 +68,18 @@ if GetOption('test'): # build translation files -translation_sources = Glob("#selfdrive/ui/translations/*.ts", strings=True) +with open(File("translations/languages.json").abspath) as f: + languages = json.loads(f.read()) +translation_sources = [f"#selfdrive/ui/translations/{l}.ts" for l in languages.values()] translation_targets = [src.replace(".ts", ".qm") for src in translation_sources] -lrelease = 'third_party/qt5/larch64/bin/lrelease' if arch == 'larch64' else 'lrelease' -qt_env.Command(translation_targets, translation_sources, f"{lrelease} $SOURCES") - +lrelease_bin = 'third_party/qt5/larch64/bin/lrelease' if arch == 'larch64' else 'lrelease' + +lupdate = qt_env.Command(translation_sources, qt_src, "selfdrive/ui/update_translations.py") +lrelease = qt_env.Command(translation_targets, translation_sources, f"{lrelease_bin} $SOURCES") +qt_env.Depends(lrelease, lupdate) +qt_env.NoClean(translation_sources) +qt_env.Precious(translation_sources) +qt_env.NoCache(lupdate) # setup and factory resetter if GetOption('extras'): diff --git a/selfdrive/ui/installer/installer.cc b/selfdrive/ui/installer/installer.cc index 7d8bbf74e0..1af72c04df 100644 --- a/selfdrive/ui/installer/installer.cc +++ b/selfdrive/ui/installer/installer.cc @@ -141,9 +141,9 @@ void Installer::cachedFetch(const QString &cache) { void Installer::readProgress() { const QVector> stages = { // prefix, weight in percentage - {tr("Receiving objects: "), 91}, - {tr("Resolving deltas: "), 2}, - {tr("Updating files: "), 7}, + {"Receiving objects: ", 91}, + {"Resolving deltas: ", 2}, + {"Updating files: ", 7}, }; auto line = QString(proc.readAllStandardError()); diff --git a/selfdrive/ui/qt/maps/map.cc b/selfdrive/ui/qt/maps/map.cc index c625564f1d..dd30b5feff 100644 --- a/selfdrive/ui/qt/maps/map.cc +++ b/selfdrive/ui/qt/maps/map.cc @@ -125,7 +125,8 @@ void MapWindow::updateState(const UIState &s) { } } - if (sm.updated("gnssMeasurements")) { + // TODO should check a valid/status flag + if (sm.updated("gnssMeasurements") && sm["gnssMeasurements"].getGnssMeasurements().getGpsWeek() > 0){ auto laikad_location = sm["gnssMeasurements"].getGnssMeasurements(); auto laikad_pos = laikad_location.getPositionECEF(); auto laikad_pos_ecef = laikad_pos.getValue(); diff --git a/selfdrive/ui/qt/offroad/settings.cc b/selfdrive/ui/qt/offroad/settings.cc index bde8628dc4..63b87149d4 100644 --- a/selfdrive/ui/qt/offroad/settings.cc +++ b/selfdrive/ui/qt/offroad/settings.cc @@ -132,16 +132,17 @@ void TogglesPanel::updateToggles() { .arg(tr("New Driving Visualization")) .arg(tr("The driving visualization will transition to the road-facing wide-angle camera at low speeds to better show some turns. The Experimental mode logo will also be shown in the top right corner.")); + const bool is_release = params.getBool("IsReleaseBranch"); auto cp_bytes = params.get("CarParamsPersistent"); if (!cp_bytes.empty()) { AlignedBuffer aligned_buf; capnp::FlatArrayMessageReader cmsg(aligned_buf.align(cp_bytes.data(), cp_bytes.size())); cereal::CarParams::Reader CP = cmsg.getRoot(); - if (!CP.getExperimentalLongitudinalAvailable()) { + if (!CP.getExperimentalLongitudinalAvailable() || is_release) { params.remove("ExperimentalLongitudinalEnabled"); } - op_long_toggle->setVisible(CP.getExperimentalLongitudinalAvailable()); + op_long_toggle->setVisible(CP.getExperimentalLongitudinalAvailable() && !is_release); const bool op_long = CP.getOpenpilotLongitudinalControl() && !CP.getExperimentalLongitudinalAvailable(); const bool exp_long_enabled = CP.getExperimentalLongitudinalAvailable() && params.getBool("ExperimentalLongitudinalEnabled"); @@ -154,9 +155,18 @@ void TogglesPanel::updateToggles() { e2e_toggle->setEnabled(false); params.remove("ExperimentalMode"); - const QString no_long = tr("Experimental mode is currently unavailable on this car, since the car's stock ACC is used for longitudinal control."); - const QString exp_long = tr("Enable experimental longitudinal control to allow experimental mode."); - e2e_toggle->setDescription("" + (CP.getExperimentalLongitudinalAvailable() ? exp_long : no_long) + "

" + e2e_description); + const QString unavailable = tr("Experimental mode is currently unavailable on this car since the car's stock ACC is used for longitudinal control."); + + QString long_desc = unavailable + " " + \ + tr("openpilot longitudinal control may come in a future update."); + if (CP.getExperimentalLongitudinalAvailable()) { + if (is_release) { + long_desc = unavailable + " " + tr("An experimental version of openpilot longitudinal control can be tested, along with Experimental mode, on non-release branches."); + } else { + long_desc = tr("Enable experimental longitudinal control to allow Experimental mode."); + } + } + e2e_toggle->setDescription("" + long_desc + "

" + e2e_description); } e2e_toggle->refresh(); diff --git a/selfdrive/ui/qt/offroad/software_settings.cc b/selfdrive/ui/qt/offroad/software_settings.cc index 12d62e63fb..6db6a6cdfe 100644 --- a/selfdrive/ui/qt/offroad/software_settings.cc +++ b/selfdrive/ui/qt/offroad/software_settings.cc @@ -54,7 +54,7 @@ SoftwarePanel::SoftwarePanel(QWidget* parent) : ListWidget(parent) { connect(targetBranchBtn, &ButtonControl::clicked, [=]() { auto current = params.get("GitBranch"); QStringList branches = QString::fromStdString(params.get("UpdaterAvailableBranches")).split(","); - for (QString b : {current.c_str(), "devel-staging", "devel", "master-ci", "master"}) { + for (QString b : {current.c_str(), "devel-staging", "devel", "nightly", "master-ci", "master"}) { auto i = branches.indexOf(b); if (i >= 0) { branches.removeAt(i); diff --git a/selfdrive/ui/qt/onroad.cc b/selfdrive/ui/qt/onroad.cc index 1b4b880fbf..33b1ea8e27 100644 --- a/selfdrive/ui/qt/onroad.cc +++ b/selfdrive/ui/qt/onroad.cc @@ -175,12 +175,63 @@ void OnroadAlerts::paintEvent(QPaintEvent *event) { } +ExperimentalButton::ExperimentalButton(QWidget *parent) : QPushButton(parent) { + setVisible(false); + setFixedSize(btn_size, btn_size); + setCheckable(true); + + params = Params(); + engage_img = loadPixmap("../assets/img_chffr_wheel.png", {img_size, img_size}); + experimental_img = loadPixmap("../assets/img_experimental.svg", {img_size, img_size}); + + QObject::connect(this, &QPushButton::toggled, [=](bool checked) { + params.putBool("ExperimentalMode", checked); + }); +} + +void ExperimentalButton::updateState(const UIState &s) { + const SubMaster &sm = *(s.sm); + + // button is "visible" if engageable or enabled + const auto cs = sm["controlsState"].getControlsState(); + setVisible(cs.getEngageable() || cs.getEnabled()); + + // button is "checked" if experimental mode is enabled + setChecked(sm["controlsState"].getControlsState().getExperimentalMode()); + + // disable button when experimental mode is not available, or has not been confirmed for the first time + const auto cp = sm["carParams"].getCarParams(); + const bool experimental_mode_available = cp.getExperimentalLongitudinalAvailable() ? params.getBool("ExperimentalLongitudinalEnabled") : cp.getOpenpilotLongitudinalControl(); + setEnabled(params.getBool("ExperimentalModeConfirmed") && experimental_mode_available); +} + +void ExperimentalButton::paintEvent(QPaintEvent *event) { + QPainter p(this); + p.setRenderHint(QPainter::Antialiasing); + + QPoint center(btn_size / 2, btn_size / 2); + QPixmap img = isChecked() ? experimental_img : engage_img; + + p.setOpacity(1.0); + p.setPen(Qt::NoPen); + p.setBrush(QColor(0, 0, 0, 166)); + p.drawEllipse(center, btn_size / 2, btn_size / 2); + p.setOpacity(isDown() ? 0.8 : 1.0); + p.drawPixmap((btn_size - img_size) / 2, (btn_size - img_size) / 2, img); +} + + AnnotatedCameraWidget::AnnotatedCameraWidget(VisionStreamType type, QWidget* parent) : fps_filter(UI_FREQ, 3, 1. / UI_FREQ), CameraWidget("camerad", type, true, parent) { pm = std::make_unique>({"uiDebug"}); - engage_img = loadPixmap("../assets/img_chffr_wheel.png", {img_size, img_size}); - experimental_img = loadPixmap("../assets/img_experimental.svg", {img_size - 5, img_size - 5}); - dm_img = loadPixmap("../assets/img_driver_face.png", {img_size, img_size}); + QVBoxLayout *main_layout = new QVBoxLayout(this); + main_layout->setMargin(bdr_s); + main_layout->setSpacing(0); + + experimental_btn = new ExperimentalButton(this); + main_layout->addWidget(experimental_btn, 0, Qt::AlignTop | Qt::AlignRight); + + dm_img = loadPixmap("../assets/img_driver_face.png", {img_size + 5, img_size + 5}); } void AnnotatedCameraWidget::updateState(const UIState &s) { @@ -224,15 +275,20 @@ void AnnotatedCameraWidget::updateState(const UIState &s) { setProperty("speed", cur_speed); setProperty("setSpeed", set_speed); setProperty("speedUnit", s.scene.is_metric ? tr("km/h") : tr("mph")); - setProperty("hideDM", cs.getAlertSize() != cereal::ControlsState::AlertSize::NONE); + setProperty("hideDM", (cs.getAlertSize() != cereal::ControlsState::AlertSize::NONE)); setProperty("status", s.status); - // update engageability and DM icons at 2Hz + // update engageability/experimental mode button + experimental_btn->updateState(s); + + // update DM icons at 2Hz if (sm.frame % (UI_FREQ / 2) == 0) { - setProperty("engageable", cs.getEngageable() || cs.getEnabled()); setProperty("dmActive", sm["driverMonitoringState"].getDriverMonitoringState().getIsActiveMode()); setProperty("rightHandDM", sm["driverMonitoringState"].getDriverMonitoringState().getIsRHD()); } + + // DM icon transition + dm_fade_state = fmax(0.0, fmin(1.0, dm_fade_state+0.2*(0.5-(float)(dmActive)))); } void AnnotatedCameraWidget::drawHud(QPainter &p) { @@ -382,19 +438,6 @@ void AnnotatedCameraWidget::drawHud(QPainter &p) { configFont(p, "Inter", 66, "Regular"); drawText(p, rect().center().x(), 290, speedUnit, 200); - // engage-ability icon - if (engageable) { - SubMaster &sm = *(uiState()->sm); - drawIcon(p, rect().right() - radius / 2 - bdr_s * 2, radius / 2 + int(bdr_s * 1.5), - sm["controlsState"].getControlsState().getExperimentalMode() ? experimental_img : engage_img, blackColor(166), 1.0); - } - - // dm icon - if (!hideDM) { - int dm_icon_x = rightHandDM ? rect().right() - radius / 2 - (bdr_s * 2) : radius / 2 + (bdr_s * 2); - drawIcon(p, dm_icon_x, rect().bottom() - footer_h / 2, - dm_img, blackColor(70), dmActive ? 1.0 : 0.2); - } p.restore(); } @@ -414,7 +457,7 @@ void AnnotatedCameraWidget::drawIcon(QPainter &p, int x, int y, QPixmap &img, QB p.setOpacity(1.0); // bg dictates opacity of ellipse p.setPen(Qt::NoPen); p.setBrush(bg); - p.drawEllipse(x - radius / 2, y - radius / 2, radius, radius); + p.drawEllipse(x - btn_size / 2, y - btn_size / 2, btn_size, btn_size); p.setOpacity(opacity); p.drawPixmap(x - img.size().width() / 2, y - img.size().height() / 2, img); } @@ -498,6 +541,49 @@ void AnnotatedCameraWidget::drawLaneLines(QPainter &painter, const UIState *s) { painter.restore(); } +void AnnotatedCameraWidget::drawDriverState(QPainter &painter, const UIState *s) { + const UIScene &scene = s->scene; + + painter.save(); + + // base icon + int x = rightHandDM ? rect().right() - (btn_size - 24) / 2 - (bdr_s * 2) : (btn_size - 24) / 2 + (bdr_s * 2); + int y = rect().bottom() - footer_h / 2; + float opacity = dmActive ? 0.65 : 0.2; + drawIcon(painter, x, y, dm_img, blackColor(0), opacity); + + // circle background + painter.setOpacity(1.0); + painter.setPen(Qt::NoPen); + painter.setBrush(blackColor(70)); + painter.drawEllipse(x - btn_size / 2, y - btn_size / 2, btn_size, btn_size); + + // face + QPointF face_kpts_draw[std::size(default_face_kpts_3d)]; + float kp; + for (int i = 0; i < std::size(default_face_kpts_3d); ++i) { + kp = (scene.face_kpts_draw[i].v[2] - 8) / 120 + 1.0; + face_kpts_draw[i] = QPointF(scene.face_kpts_draw[i].v[0] * kp + x, scene.face_kpts_draw[i].v[1] * kp + y); + } + + painter.setPen(QPen(QColor::fromRgbF(1.0, 1.0, 1.0, opacity), 5.2, Qt::SolidLine, Qt::RoundCap)); + painter.drawPolyline(face_kpts_draw, std::size(default_face_kpts_3d)); + + // tracking arcs + const int arc_l = 133; + const float arc_t_default = 6.7; + const float arc_t_extend = 12.0; + QColor arc_color = QColor::fromRgbF(0.09, 0.945, 0.26, 0.4*(1.0-dm_fade_state)*(s->engaged())); + float delta_x = -scene.driver_pose_sins[1] * arc_l / 2; + float delta_y = -scene.driver_pose_sins[0] * arc_l / 2; + painter.setPen(QPen(arc_color, arc_t_default+arc_t_extend*fmin(1.0, scene.driver_pose_diff[1] * 5.0), Qt::SolidLine, Qt::RoundCap)); + painter.drawArc(QRectF(std::fmin(x + delta_x, x), y - arc_l / 2, fabs(delta_x), arc_l), (scene.driver_pose_sins[1]>0 ? 90 : -90) * 16, 180 * 16); + painter.setPen(QPen(arc_color, arc_t_default+arc_t_extend*fmin(1.0, scene.driver_pose_diff[0] * 5.0), Qt::SolidLine, Qt::RoundCap)); + painter.drawArc(QRectF(x - arc_l / 2, std::fmin(y + delta_y, y), arc_l, fabs(delta_y)), (scene.driver_pose_sins[0]>0 ? 0 : 180) * 16, 180 * 16); + + painter.restore(); +} + void AnnotatedCameraWidget::drawLead(QPainter &painter, const cereal::RadarState::LeadData::Reader &lead_data, const QPointF &vd) { painter.save(); @@ -608,6 +694,12 @@ void AnnotatedCameraWidget::paintGL() { } } + // DMoji + if (!hideDM && (sm.rcv_frame("driverStateV2") > s->scene.started_frame)) { + update_dmonitoring(s, sm["driverStateV2"].getDriverStateV2(), dm_fade_state, rightHandDM); + drawDriverState(painter, s); + } + drawHud(painter); double cur_draw_t = millis_since_boot(); diff --git a/selfdrive/ui/qt/onroad.h b/selfdrive/ui/qt/onroad.h index 3dbb05b674..73c2a37892 100644 --- a/selfdrive/ui/qt/onroad.h +++ b/selfdrive/ui/qt/onroad.h @@ -1,11 +1,16 @@ #pragma once +#include #include #include #include "common/util.h" -#include "selfdrive/ui/qt/widgets/cameraview.h" #include "selfdrive/ui/ui.h" +#include "selfdrive/ui/qt/widgets/cameraview.h" + + +const int btn_size = 192; +const int img_size = (btn_size / 4) * 3; // ***** onroad widgets ***** @@ -24,6 +29,21 @@ private: Alert alert = {}; }; +class ExperimentalButton : public QPushButton { + Q_OBJECT + +public: + explicit ExperimentalButton(QWidget *parent = 0); + void updateState(const UIState &s); + +private: + void paintEvent(QPaintEvent *event) override; + + Params params; + QPixmap engage_img; + QPixmap experimental_img; +}; + // container window for the NVG UI class AnnotatedCameraWidget : public CameraWidget { Q_OBJECT @@ -36,7 +56,6 @@ class AnnotatedCameraWidget : public CameraWidget { Q_PROPERTY(bool has_us_speed_limit MEMBER has_us_speed_limit); Q_PROPERTY(bool is_metric MEMBER is_metric); - Q_PROPERTY(bool engageable MEMBER engageable); Q_PROPERTY(bool dmActive MEMBER dmActive); Q_PROPERTY(bool hideDM MEMBER hideDM); Q_PROPERTY(bool rightHandDM MEMBER rightHandDM); @@ -50,21 +69,18 @@ private: void drawIcon(QPainter &p, int x, int y, QPixmap &img, QBrush bg, float opacity); void drawText(QPainter &p, int x, int y, const QString &text, int alpha = 255); - QPixmap engage_img; - QPixmap experimental_img; + ExperimentalButton *experimental_btn; QPixmap dm_img; - const int radius = 192; - const int img_size = (radius / 2) * 1.5; float speed; QString speedUnit; float setSpeed; float speedLimit; bool is_cruise_set = false; bool is_metric = false; - bool engageable = false; bool dmActive = false; bool hideDM = false; bool rightHandDM = false; + float dm_fade_state = 1.0; bool has_us_speed_limit = false; bool has_eu_speed_limit = false; bool v_ego_cluster_seen = false; @@ -82,6 +98,7 @@ protected: void drawLaneLines(QPainter &painter, const UIState *s); void drawLead(QPainter &painter, const cereal::RadarState::LeadData::Reader &lead_data, const QPointF &vd); void drawHud(QPainter &p); + void drawDriverState(QPainter &painter, const UIState *s); inline QColor redColor(int alpha = 255) { return QColor(201, 34, 49, alpha); } inline QColor whiteColor(int alpha = 255) { return QColor(255, 255, 255, alpha); } inline QColor blackColor(int alpha = 255) { return QColor(0, 0, 0, alpha); } diff --git a/selfdrive/ui/qt/setup/setup.cc b/selfdrive/ui/qt/setup/setup.cc index 69dafcf741..392be68a12 100644 --- a/selfdrive/ui/qt/setup/setup.cc +++ b/selfdrive/ui/qt/setup/setup.cc @@ -20,6 +20,17 @@ const std::string USER_AGENT = "AGNOSSetup-"; const QString DASHCAM_URL = "https://dashcam.comma.ai"; +bool is_elf(char *fname) { + FILE *fp = fopen(fname, "rb"); + if (fp == NULL) { + return false; + } + char buf[4]; + size_t n = fread(buf, 1, 4, fp); + fclose(fp); + return n == 4 && buf[0] == 0x7f && buf[1] == 'E' && buf[2] == 'L' && buf[3] == 'F'; +} + void Setup::download(QString url) { CURL *curl = curl_easy_init(); if (!curl) { @@ -29,6 +40,9 @@ void Setup::download(QString url) { auto version = util::read_file("/VERSION"); + struct curl_slist *list = NULL; + list = curl_slist_append(list, ("X-openpilot-serial: " + Hardware::get_serial()).c_str()); + char tmpfile[] = "/tmp/installer_XXXXXX"; FILE *fp = fdopen(mkstemp(tmpfile), "w"); @@ -38,18 +52,19 @@ void Setup::download(QString url) { curl_easy_setopt(curl, CURLOPT_VERBOSE, 1L); curl_easy_setopt(curl, CURLOPT_FOLLOWLOCATION, 1L); curl_easy_setopt(curl, CURLOPT_USERAGENT, (USER_AGENT + version).c_str()); + curl_easy_setopt(curl, CURLOPT_HTTPHEADER, list); int ret = curl_easy_perform(curl); - long res_status = 0; curl_easy_getinfo(curl, CURLINFO_RESPONSE_CODE, &res_status); - if (ret == CURLE_OK && res_status == 200) { + if (ret == CURLE_OK && res_status == 200 && is_elf(tmpfile)) { rename(tmpfile, "/tmp/installer"); emit finished(true); } else { emit finished(false); } + curl_slist_free_all(list); curl_easy_cleanup(curl); fclose(fp); } @@ -170,7 +185,20 @@ QWidget * Setup::network_setup() { QPushButton *cont = new QPushButton(); cont->setObjectName("navBtn"); cont->setProperty("primary", true); - QObject::connect(cont, &QPushButton::clicked, this, &Setup::nextPage); + QObject::connect(cont, &QPushButton::clicked, [=]() { + auto w = currentWidget(); + QTimer::singleShot(0, [=]() { + setCurrentWidget(downloading_widget); + }); + QString url = InputDialog::getText(tr("Enter URL"), this, tr("for Custom Software")); + if (!url.isEmpty()) { + QTimer::singleShot(1000, this, [=]() { + download(url); + }); + } else { + setCurrentWidget(w); + } + }); blayout->addWidget(cont); // setup timer for testing internet connection @@ -178,8 +206,8 @@ QWidget * Setup::network_setup() { QObject::connect(request, &HttpRequest::requestDone, [=](const QString &, bool success) { cont->setEnabled(success); if (success) { - const bool cell = networking->wifi->currentNetworkType() == NetworkType::CELL; - cont->setText(cell ? tr("Continue without Wi-Fi") : tr("Continue")); + const bool wifi = networking->wifi->currentNetworkType() == NetworkType::WIFI; + cont->setText(wifi ? tr("Continue") : tr("Continue without Wi-Fi")); } else { cont->setText(tr("Waiting for internet")); } @@ -197,106 +225,6 @@ QWidget * Setup::network_setup() { return widget; } -QWidget * radio_button(QString title, QButtonGroup *group) { - QPushButton *btn = new QPushButton(title); - btn->setCheckable(true); - group->addButton(btn); - btn->setStyleSheet(R"( - QPushButton { - height: 230; - padding-left: 100px; - padding-right: 100px; - text-align: left; - font-size: 80px; - font-weight: 400; - border-radius: 10px; - background-color: #4F4F4F; - } - QPushButton:checked { - background-color: #465BEA; - } - )"); - - // checkmark icon - QPixmap pix(":/img_circled_check.svg"); - btn->setIcon(pix); - btn->setIconSize(QSize(0, 0)); - btn->setLayoutDirection(Qt::RightToLeft); - QObject::connect(btn, &QPushButton::toggled, [=](bool checked) { - btn->setIconSize(checked ? QSize(104, 104) : QSize(0, 0)); - }); - return btn; -} - -QWidget * Setup::software_selection() { - QWidget *widget = new QWidget(); - QVBoxLayout *main_layout = new QVBoxLayout(widget); - main_layout->setContentsMargins(55, 50, 55, 50); - main_layout->setSpacing(0); - - // title - QLabel *title = new QLabel(tr("Choose Software to Install")); - title->setStyleSheet("font-size: 90px; font-weight: 500;"); - main_layout->addWidget(title, 0, Qt::AlignLeft | Qt::AlignTop); - - main_layout->addSpacing(50); - - // dashcam + custom radio buttons - QButtonGroup *group = new QButtonGroup(widget); - group->setExclusive(true); - - QWidget *dashcam = radio_button(tr("Dashcam"), group); - main_layout->addWidget(dashcam); - - main_layout->addSpacing(30); - - QWidget *custom = radio_button(tr("Custom Software"), group); - main_layout->addWidget(custom); - - main_layout->addStretch(); - - // back + continue buttons - QHBoxLayout *blayout = new QHBoxLayout; - main_layout->addLayout(blayout); - blayout->setSpacing(50); - - QPushButton *back = new QPushButton(tr("Back")); - back->setObjectName("navBtn"); - QObject::connect(back, &QPushButton::clicked, this, &Setup::prevPage); - blayout->addWidget(back); - - QPushButton *cont = new QPushButton(tr("Continue")); - cont->setObjectName("navBtn"); - cont->setEnabled(false); - cont->setProperty("primary", true); - blayout->addWidget(cont); - - QObject::connect(cont, &QPushButton::clicked, [=]() { - auto w = currentWidget(); - QTimer::singleShot(0, [=]() { - setCurrentWidget(downloading_widget); - }); - QString url = DASHCAM_URL; - if (group->checkedButton() != dashcam) { - url = InputDialog::getText(tr("Enter URL"), this, tr("for Custom Software")); - } - if (!url.isEmpty()) { - QTimer::singleShot(1000, this, [=]() { - download(url); - }); - } else { - setCurrentWidget(w); - } - }); - - connect(group, QOverload::of(&QButtonGroup::buttonClicked), [=](QAbstractButton *btn) { - btn->setChecked(true); - cont->setEnabled(true); - }); - - return widget; -} - QWidget * Setup::downloading() { QWidget *widget = new QWidget(); QVBoxLayout *main_layout = new QVBoxLayout(widget); @@ -372,7 +300,6 @@ Setup::Setup(QWidget *parent) : QStackedWidget(parent) { addWidget(getting_started()); addWidget(network_setup()); - addWidget(software_selection()); downloading_widget = downloading(); addWidget(downloading_widget); diff --git a/selfdrive/ui/qt/setup/setup.h b/selfdrive/ui/qt/setup/setup.h index 8027e8bd4f..f990b5a6cb 100644 --- a/selfdrive/ui/qt/setup/setup.h +++ b/selfdrive/ui/qt/setup/setup.h @@ -14,7 +14,6 @@ private: QWidget *low_voltage(); QWidget *getting_started(); QWidget *network_setup(); - QWidget *software_selection(); QWidget *downloading(); QWidget *download_failed(); diff --git a/selfdrive/ui/qt/util.cc b/selfdrive/ui/qt/util.cc index 59903e3376..4f52310649 100644 --- a/selfdrive/ui/qt/util.cc +++ b/selfdrive/ui/qt/util.cc @@ -2,11 +2,14 @@ #include #include +#include #include #include #include #include #include +#include +#include #include "common/params.h" #include "common/swaglog.h" @@ -218,3 +221,37 @@ QColor interpColor(float xv, std::vector xp, std::vector fp) { ); } } + +static QHash load_bootstrap_icons() { + QHash icons; + + QFile f(":/bootstrap-icons.svg"); + if (f.open(QIODevice::ReadOnly | QIODevice::Text)) { + QDomDocument xml; + xml.setContent(&f); + QDomNode n = xml.documentElement().firstChild(); + while (!n.isNull()) { + QDomElement e = n.toElement(); + if (!e.isNull() && e.hasAttribute("id")) { + QString svg_str; + QTextStream stream(&svg_str); + n.save(stream, 0); + svg_str.replace("", ""); + icons[e.attribute("id")] = svg_str.toUtf8(); + } + n = n.nextSibling(); + } + } + return icons; +} + +QPixmap bootstrapPixmap(const QString &id) { + static QHash icons = load_bootstrap_icons(); + + QPixmap pixmap; + if (auto it = icons.find(id); it != icons.end()) { + pixmap.loadFromData(it.value(), "svg"); + } + return pixmap; +} diff --git a/selfdrive/ui/qt/util.h b/selfdrive/ui/qt/util.h index 61a27a8669..3188f3f9b9 100644 --- a/selfdrive/ui/qt/util.h +++ b/selfdrive/ui/qt/util.h @@ -22,6 +22,7 @@ void swagLogMessageHandler(QtMsgType type, const QMessageLogContext &context, co void initApp(int argc, char *argv[]); QWidget* topWidget (QWidget* widget); QPixmap loadPixmap(const QString &fileName, const QSize &size = {}, Qt::AspectRatioMode aspectRatioMode = Qt::KeepAspectRatio); +QPixmap bootstrapPixmap(const QString &id); QRect getTextRect(QPainter &p, int flags, const QString &text); void drawRoundedRect(QPainter &painter, const QRectF &rect, qreal xRadiusTop, qreal yRadiusTop, qreal xRadiusBottom, qreal yRadiusBottom); diff --git a/selfdrive/ui/qt/widgets/cameraview.cc b/selfdrive/ui/qt/widgets/cameraview.cc index 16aad13437..8c7a7072e2 100644 --- a/selfdrive/ui/qt/widgets/cameraview.cc +++ b/selfdrive/ui/qt/widgets/cameraview.cc @@ -358,7 +358,7 @@ void CameraWidget::vipcThread() { while (!QThread::currentThread()->isInterruptionRequested()) { if (!vipc_client || cur_stream != requested_stream_type) { clearFrames(); - qDebug() << "connecting to stream " << requested_stream_type << ", was connected to " << cur_stream; + qDebug().nospace() << "connecting to stream " << requested_stream_type << ", was connected to " << cur_stream; cur_stream = requested_stream_type; vipc_client.reset(new VisionIpcClient(stream_name, cur_stream, false)); } @@ -382,6 +382,10 @@ void CameraWidget::vipcThread() { } } emit vipcThreadFrameReceived(); + } else { + if (!isVisible()) { + vipc_client->connected = false; + } } } diff --git a/selfdrive/ui/translations/main_de.ts b/selfdrive/ui/translations/main_de.ts index ecb44557c9..b40812af7c 100644 --- a/selfdrive/ui/translations/main_de.ts +++ b/selfdrive/ui/translations/main_de.ts @@ -312,18 +312,6 @@ Installing... Installiere... - - Receiving objects: - Empfange Objekte: - - - Resolving deltas: - Unterschiede verarbeiten: - - - Updating files: - Dateien aktualisieren: - MapETA @@ -686,18 +674,6 @@ location set Waiting for internet Auf Internet warten - - Choose Software to Install - Software zum installieren auswählen - - - Dashcam - Dashcam - - - Custom Software - Spezifische Software - Enter URL URL eingeben @@ -1044,11 +1020,19 @@ location set Die Fahrvisualisierung wechselt bei niedrigen Geschwindigkeiten zur Straßengewandten Weitwinkelkamera, um manche Kurven besser zu zeigen. Außerdem wird das Experimenteller Modus logo oben rechts angezeigt. - Experimental mode is currently unavailable on this car, since the car's stock ACC is used for longitudinal control. - Der experimentelle Modus ist momentan für dieses Auto nicht verfügbar, da es den eingebauten adaptiven Tempomaten des Autos benutzt. + Experimental mode is currently unavailable on this car since the car's stock ACC is used for longitudinal control. + Der experimentelle Modus ist momentan für dieses Auto nicht verfügbar da es den eingebauten adaptiven Tempomaten des Autos benutzt. + + + openpilot longitudinal control may come in a future update. + + + + An experimental version of openpilot longitudinal control can be tested, along with Experimental mode, on non-release branches. + - Enable experimental longitudinal control to allow experimental mode. + Enable experimental longitudinal control to allow Experimental mode. Aktiviere den experimentellen Openpilot Tempomaten für experimentelle Funktionen. diff --git a/selfdrive/ui/translations/main_ja.ts b/selfdrive/ui/translations/main_ja.ts index 8226dd59f9..ed08a52df5 100644 --- a/selfdrive/ui/translations/main_ja.ts +++ b/selfdrive/ui/translations/main_ja.ts @@ -311,18 +311,6 @@ Installing... インストールしています... - - Receiving objects: - ｵﾌﾞｼﾞｪｸﾄをﾀﾞｳﾝﾛｰﾄﾞしています： - - - Resolving deltas: - デルタを解決しています： - - - Updating files: - ファイルを更新しています： - MapETA @@ -684,18 +672,6 @@ location set Waiting for internet インターネット接続を待機中 - - Choose Software to Install - インストールするソフトウェアを選択してください - - - Dashcam - ﾄﾞﾗｲﾌﾞﾚｺｰﾀﾞｰ - - - Custom Software - カスタムソフトウェア - Enter URL URL を入力 @@ -1017,14 +993,6 @@ location set On this car, openpilot defaults to the car's built-in ACC instead of openpilot's longitudinal control. Enable this to switch to openpilot longitudinal control. Enabling Experimental mode is recommended when using experimental openpilot longitudinal control. openpilotはこの車の場合、車に内蔵されているACCを標準で利用します。この機能を有効にすることで実験段階のopenpilotによるアクセル制御を利用できます。実験モードと合わせて利用することをお勧めします。 - - Experimental mode is currently unavailable on this car, since the car's stock ACC is used for longitudinal control. - この車のACCがアクセル制御を行うため、実験モードを利用することができません。 - - - Enable experimental longitudinal control to allow experimental mode. - 実験段階のopenpilotによるアクセル制御を有効にしてください。 - openpilot defaults to driving in <b>chill mode</b>. Experimental mode enables <b>alpha-level features</b> that aren't ready for chill mode. Experimental features are listed below: openpilotは標準ではゆっくりとくつろげる運転を提供します。この実験モードを有効にすると、以下のくつろげる段階ではない開発中の機能を利用する事ができます。 @@ -1045,6 +1013,22 @@ location set The driving visualization will transition to the road-facing wide-angle camera at low speeds to better show some turns. The Experimental mode logo will also be shown in the top right corner. 新しい運転画面では、低速時に広角カメラの映像を表示することで、曲がる際の道路の視覚を向上します。実験段階を表すマークが右上に表示されます。 + + Experimental mode is currently unavailable on this car since the car's stock ACC is used for longitudinal control. + この車のACCがアクセル制御を行うため実験モードを利用することができません。 + + + openpilot longitudinal control may come in a future update. + + + + An experimental version of openpilot longitudinal control can be tested, along with Experimental mode, on non-release branches. + + + + Enable experimental longitudinal control to allow Experimental mode. + 実験段階のopenpilotによるアクセル制御を有効にしてください。 + Updater diff --git a/selfdrive/ui/translations/main_ko.ts b/selfdrive/ui/translations/main_ko.ts index bda64c53ab..7b742a5048 100644 --- a/selfdrive/ui/translations/main_ko.ts +++ b/selfdrive/ui/translations/main_ko.ts @@ -311,18 +311,6 @@ Installing... 설치중... - - Receiving objects: - 수신중： - - - Resolving deltas: - 델타병합： - - - Updating files: - 파일갱신： - MapETA @@ -501,7 +489,7 @@ location set Become a comma prime member at connect.comma.ai - connect.comma.ai에서 comma prime에 가입합니다 + connect.comma.ai 접속 comma prime 가입 PRIME FEATURES: @@ -684,18 +672,6 @@ location set Waiting for internet 네트워크 접속을 기다립니다 - - Choose Software to Install - 설치할 소프트웨어를 선택하세요 - - - Dashcam - Dashcam - - - Custom Software - Custom Software - Enter URL URL 입력 @@ -1017,17 +993,9 @@ location set On this car, openpilot defaults to the car's built-in ACC instead of openpilot's longitudinal control. Enable this to switch to openpilot longitudinal control. Enabling Experimental mode is recommended when using experimental openpilot longitudinal control. 이 차량은 openpilot 롱컨트롤 대신 차량의 내장 ACC로 기본 설정됩니다. openpilot 롱컨트롤을 사용하려면 이 옵션을 활성화하세요. 실험적 openpilot 롱컨트롤을 사용하는 경우 실험적 모드를 활성화 하세요. - - Experimental mode is currently unavailable on this car, since the car's stock ACC is used for longitudinal control. - 차량의 기본 ACC가 롱컨트롤에 사용되기 때문에 현재 이 차량에서는 실험적 모드를 사용할수 없습니다. - - - Enable experimental longitudinal control to allow experimental mode. - 실험적 롱컨트롤을 사용하려면 실험적 모드를 활성화 하세요. - openpilot defaults to driving in <b>chill mode</b>. Experimental mode enables <b>alpha-level features</b> that aren't ready for chill mode. Experimental features are listed below: - openpilot은 기본적으로 <b>안정적 모드</b>로 주행합니다. 실험적 모드는 안정적 모드에 준비되지 않은 <b>알파 수준 기능</b>을 활성화 합니다. 실험 모드의 특징은 아래에 나열되어 있습니다 + openpilot은 기본적으로 <b>안정적 모드</b>로 주행합니다. 실험적 모드는 안정적 모드에 준비되지 않은 <b>알파 수준 기능</b>을 활성화 합니다. 실험적 모드의 특징은 아래에 나열되어 있습니다 🌮 End-to-End Longitudinal Control 🌮 @@ -1043,7 +1011,23 @@ location set The driving visualization will transition to the road-facing wide-angle camera at low speeds to better show some turns. The Experimental mode logo will also be shown in the top right corner. - 주행 시각화는 저속에서 도로를 향하는 광각 카메라로 전환되어 일부 회전을 더 잘 보여줍니다. 실험적 모드 로고도 우측상단에 표시됩니다. + 주행 시각화는 저속에서 도로를 향하는 광각 카메라로 전환되어 일부 회전을 더 잘 보여줍니다. 실험적 모드의 로고도 우측상단에 표시됩니다. + + + Experimental mode is currently unavailable on this car since the car's stock ACC is used for longitudinal control. + 차량에 장착된 ACC가 롱컨트롤에 사용되기 때문에 현재 이 차량은 실험적 모드를 사용할 수 없습니다. + + + openpilot longitudinal control may come in a future update. + 오픈파일럿 롱컨트롤은 향후 업데이트에서 제공될 수 있습니다. + + + An experimental version of openpilot longitudinal control can be tested, along with Experimental mode, on non-release branches. + 오픈파일럿 롱컨트롤의 실험 버전은 실험적 모드와 함께 릴리즈 되지 않은 브랜치에서 테스트할 수 있습니다. + + + Enable experimental longitudinal control to allow Experimental mode. + 실험적 롱컨트롤을 사용하려면 실험적 모드를 활성화 하세요. diff --git a/selfdrive/ui/translations/main_pt-BR.ts b/selfdrive/ui/translations/main_pt-BR.ts index 105d6f77f0..a25c51de3c 100644 --- a/selfdrive/ui/translations/main_pt-BR.ts +++ b/selfdrive/ui/translations/main_pt-BR.ts @@ -132,19 +132,19 @@ Driver Camera - Câmera voltada para o Motorista + Câmera do Motorista PREVIEW - PREVISUAL + VER Preview the driver facing camera to ensure that driver monitoring has good visibility. (vehicle must be off) - Pré-visualizar a câmera voltada para o motorista para garantir que monitor tem uma boa visibilidade (veículo precisa estar desligado) + Pré-visualizar a câmera voltada para o motorista para garantir que o monitoramento do sistema tenha uma boa visibilidade (veículo precisa estar desligado) Reset Calibration - Resetar Calibragem + Reinicializar Calibragem RESET @@ -200,7 +200,7 @@ openpilot requires the device to be mounted within 4° left or right and within 5° up or 8° down. openpilot is continuously calibrating, resetting is rarely required. - o openpilot requer que o dispositivo seja montado dentro de 4° esquerda ou direita e dentro de 5° para cima ou 8° para baixo. o openpilot está continuamente calibrando, resetar raramente é necessário. + O openpilot requer que o dispositivo seja montado dentro de 4° esquerda ou direita e dentro de 5° para cima ou 8° para baixo. O openpilot está continuamente calibrando, resetar raramente é necessário. Your device is pointed %1° %2 and %3° %4. @@ -312,18 +312,6 @@ Installing... Instalando... - - Receiving objects: - Recebendo objetos: - - - Resolving deltas: - Resolvendo deltas: - - - Updating files: - Atualizando arquivos: - MapETA @@ -388,8 +376,8 @@ Get turn-by-turn directions displayed and more with a comma prime subscription. Sign up now: https://connect.comma.ai - Obtenha instruções passo a passo exibidas e muito mais com -uma assinatura prime Inscreva-se agora: https://connect.comma.ai + Obtenha instruções passo a passo exibidas e muito mais com +uma assinatura prime. Inscreva-se agora: https://connect.comma.ai No home @@ -502,7 +490,7 @@ trabalho definido Become a comma prime member at connect.comma.ai - Torne-se um membro comma prime em connect.comma.ai + Seja um membro comma prime em connect.comma.ai PRIME FEATURES: @@ -514,11 +502,11 @@ trabalho definido 1 year of storage - 1 ano de armazenamento + 1 ano na nuvem Developer perks - Benefícios para desenvolvedor + Benefícios para devs @@ -688,18 +676,6 @@ trabalho definido Waiting for internet Esperando pela internet - - Choose Software to Install - Escolher Software para Instalar - - - Dashcam - Dashcam - - - Custom Software - Sofware Customizado - Enter URL Preencher URL @@ -835,7 +811,7 @@ trabalho definido Current Version - Versao Atual + Versão Atual Download @@ -863,11 +839,11 @@ trabalho definido UNINSTALL - DESINSTAL + REMOVER Uninstall %1 - Desintalar o %1 + Desinstalar o %1 Are you sure you want to uninstall? @@ -987,7 +963,7 @@ trabalho definido Disengage on Accelerator Pedal - Desacionar Com Pedal Do Acelerador + Desacionar com Pedal do Acelerador When enabled, pressing the accelerator pedal will disengage openpilot. @@ -995,7 +971,7 @@ trabalho definido Show ETA in 24h Format - Mostrar ETA em formato 24h + Mostrar ETA em Formato 24h Use 24h format instead of am/pm @@ -1021,14 +997,6 @@ trabalho definido On this car, openpilot defaults to the car's built-in ACC instead of openpilot's longitudinal control. Enable this to switch to openpilot longitudinal control. Enabling Experimental mode is recommended when using experimental openpilot longitudinal control. Neste carro o penpilot por padrão utiliza o ACC nativo do veículo ao invés de controlar longitudinalmente. Ative isto para mudar para o controle longitudinal do openpilot. Ativar o Modo Experimental é recomendado quando em uso do controle longitudinal experimental do openpilot. - - Experimental mode is currently unavailable on this car, since the car's stock ACC is used for longitudinal control. - O Modo Experimental está atualmente indisponível para este carro, já que o ACC original do carro é usado para controle longitudinal. - - - Enable experimental longitudinal control to allow experimental mode. - Ative o controle longitudinal experimental para permitir o modo experimental. - openpilot defaults to driving in <b>chill mode</b>. Experimental mode enables <b>alpha-level features</b> that aren't ready for chill mode. Experimental features are listed below: openpilot por padrão funciona em <b>modo chill</b>. modo Experimental ativa <b>recursos de nível-alfa</b> que não estão prontos para o modo chill. Recursos experimentais estão listados abaixo: @@ -1049,6 +1017,22 @@ trabalho definido The driving visualization will transition to the road-facing wide-angle camera at low speeds to better show some turns. The Experimental mode logo will also be shown in the top right corner. A visualização da direção fará a transição para a câmera grande angular voltada para a estrada em baixas velocidades para mostrar melhor algumas curvas. O logotipo do modo Experimental também será exibido no canto superior direito. + + Experimental mode is currently unavailable on this car since the car's stock ACC is used for longitudinal control. + O modo Experimental está atualmente indisponível para este carro já que o ACC original do carro é usado para controle longitudinal. + + + openpilot longitudinal control may come in a future update. + O controle longitudinal openpilot pode vir em uma atualização futura. + + + An experimental version of openpilot longitudinal control can be tested, along with Experimental mode, on non-release branches. + Uma versão experimental do controle longitudinal openpilot pode ser testada, juntamente com o modo Experimental, em branches de desenvolvimento. + + + Enable experimental longitudinal control to allow Experimental mode. + Ative o controle longitudinal experimental para permitir o modo Experimental. + Updater diff --git a/selfdrive/ui/translations/main_th.ts b/selfdrive/ui/translations/main_th.ts index 0de0ba5f9a..ce394ecb97 100644 --- a/selfdrive/ui/translations/main_th.ts +++ b/selfdrive/ui/translations/main_th.ts @@ -238,6 +238,14 @@ Disengage to Power Off ยกเลิกระบบช่วยขับเพื่อปิดเครื่อง + + Reset + รีเซ็ต + + + Review + ทบทวน + DriveStats @@ -273,6 +281,17 @@ กำลังเปิดกล้อง + + ExperimentalModeButton + + EXPERIMENTAL MODE ON + คุณกำลังใช้โหมดทดลอง + + + CHILL MODE ON + คุณกำลังใช้โหมดชิล + + InputDialog @@ -463,6 +482,17 @@ location set จดจำ connect.comma.ai โดยการเพิ่มไปยังหน้าจอโฮม เพื่อใช้งานเหมือนเป็นแอปพลิเคชัน + + ParamControl + + Enable + เปิดใช้งาน + + + Cancel + ยกเลิก + + PrimeAdWidget @@ -585,13 +615,6 @@ location set ไม่สามารถเมานต์พาร์ติชั่นข้อมูล กดยืนยันเพื่อรีเซ็ตอุปกรณ์ของคุณ - - RichTextDialog - - Ok - ตกลง - - SettingsWindow @@ -850,6 +873,10 @@ location set Select a branch เลือก Branch + + Uninstall + ถอนการติดตั้ง + SshControl @@ -974,29 +1001,57 @@ location set Show map on left side when in split screen view. แสดงแผนที่ด้านซ้ายของหน้าจอเมื่ออยู่ในโหมดแบ่งหน้าจอ - - 🌮 End-to-end longitudinal (extremely alpha) 🌮 - 🌮 ควบคุมเร่ง/เบรคแบบ End-to-end (อยู่ขั้นพัฒนา) 🌮 - Experimental openpilot Longitudinal Control ทดลองใช้ระบบควบคุมการเร่ง/เบรคโดย openpilot - <b>WARNING: openpilot longitudinal control is experimental for this car and will disable AEB.</b> - <b>คำเตือน: การควบคุมการเร่ง/เบรคโดย openpilot สำหรับรถคันนี้ยังอยู่ในขั้นทดลอง และระบบเบรคฉุกเฉินอัตโนมัติ (AEB) จะถูกปิด</b> + Experimental Mode + โหมดทดลอง - Let the driving model control the gas and brakes. openpilot will drive as it thinks a human would. Super experimental. - ให้ openpilot ควบคุมการเร่ง/เบรคแบบ end-to-end โดย openpilot จะขับอย่างที่มนุษย์คิด ระบบยังอยู่ในขั้นทดลอง + WARNING: openpilot longitudinal control is experimental for this car and will disable Automatic Emergency Braking (AEB). + คำเตือน: การควบคุมการเร่ง/เบรคโดย openpilot สำหรับรถคันนี้ยังอยู่ในขั้นพัฒนา และระบบเบรคฉุกเฉินอัตโนมัติ (AEB) จะถูกปิด - openpilot longitudinal control is not currently available for this car. - ขณะนี้ยังไม่มีระบบควบคุมการเร่ง/เบรคโดย openpilot สำหรับรถคันนี้ + On this car, openpilot defaults to the car's built-in ACC instead of openpilot's longitudinal control. Enable this to switch to openpilot longitudinal control. Enabling Experimental mode is recommended when using experimental openpilot longitudinal control. + โดยปกติสำหรับรถคันนี้ openpilot จะควบคุมการเร่ง/เบรคด้วยระบบ ACC จากโรงงาน แทนการควยคุมโดย openpilot เปิดสวิตซ์นี้เพื่อให้ openpilot ควบคุมการเร่ง/เบรค แนะนำให้เปิดโหมดทดลองเมื่อต้องการให้ openpilot ควบคุมการเร่ง/เบรค ซึ่งอยู่ในขั้นพัฒนา - Enable experimental longitudinal control to enable this. - เปิดใช้งานระบบควบคุมการเร่ง/เบรคขั้นทดลอง เพื่อเปิดใช้งานสิ่งนี้ + openpilot defaults to driving in <b>chill mode</b>. Experimental mode enables <b>alpha-level features</b> that aren't ready for chill mode. Experimental features are listed below: + โดยปกติ openpilot จะขับใน<b>โหมดชิล</b> เปิดโหมดทดลองเพื่อใช้<b>ความสามารถในขั้นพัฒนา</b> ซึ่งยังไม่พร้อมสำหรับโหมดชิล ความสามารถในขั้นพัฒนามีดังนี้: + + + 🌮 End-to-End Longitudinal Control 🌮 + 🌮 ควบคุมเร่ง/เบรคแบบ End-to-End 🌮 + + + Let the driving model control the gas and brakes. openpilot will drive as it thinks a human would, including stopping for red lights and stop signs. Since the driving model decides the speed to drive, the set speed will only act as an upper bound. This is an alpha quality feature; mistakes should be expected. + ให้ openpilot ควบคุมการเร่ง/เบรค โดย openpilot จะขับอย่างที่มนุษย์คิด รวมถึงการหยุดที่ไฟแดง และป้ายหยุดรถ เนื่องจาก openpilot จะกำหนดความเร็วในการขับด้วยตัวเอง การตั้งความเร็วจะเป็นเพียงการกำหนดความเร็วสูงสูดเท่านั้น ความสามารถนี้ยังอยู่ในขั้นพัฒนา อาจเกิดข้อผิดพลาดขึ้นได้ + + + New Driving Visualization + การแสดงภาพการขับขี่แบบใหม่ + + + The driving visualization will transition to the road-facing wide-angle camera at low speeds to better show some turns. The Experimental mode logo will also be shown in the top right corner. + การแสดงภาพการขับขี่จะเปลี่ยนไปใช้กล้องมุมกว้างที่หันหน้าไปทางถนนเมื่ออยู่ในความเร็วต่ำ เพื่อแสดงภาพการเลี้ยวที่ดีขึ้น โลโก้โหมดการทดลองจะแสดงที่มุมบนขวาด้วย + + + Experimental mode is currently unavailable on this car since the car's stock ACC is used for longitudinal control. + ขณะนี้โหมดทดลองไม่สามารถใช้งานได้ในรถคันนี้ เนื่องจากเปิดใช้ระบบควบคุมการเร่ง/เบรคของรถที่ติดตั้งจากโรงงานอยู่ + + + openpilot longitudinal control may come in a future update. + ระบบควบคุมการเร่ง/เบรคโดย openpilot อาจมาในการอัปเดตในอนาคต + + + An experimental version of openpilot longitudinal control can be tested, along with Experimental mode, on non-release branches. + เวอร์ชันทดลองของระบบควบคุมการเร่ง/เบรคโดย openpilot สามารถทดสอบได้พร้อมกับโหมดการทดลอง บน branch ที่กำลังพัฒนา + + + Enable experimental longitudinal control to allow Experimental mode. + เปิดระบบควบคุมการเร่ง/เบรคขั้นพัฒนาโดย openpilot เพื่อเปิดใช้งานโหมดทดลอง @@ -1052,5 +1107,9 @@ location set Forget Wi-Fi Network "%1"? เลิกใช้เครือข่าย Wi-Fi "%1"? + + Forget + เลิกใช้ + diff --git a/selfdrive/ui/translations/main_zh-CHS.ts b/selfdrive/ui/translations/main_zh-CHS.ts index 31202e45f2..68c1bb766f 100644 --- a/selfdrive/ui/translations/main_zh-CHS.ts +++ b/selfdrive/ui/translations/main_zh-CHS.ts @@ -311,18 +311,6 @@ Installing... 正在安装…… - - Receiving objects: - 正在接收： - - - Resolving deltas: - 正在处理： - - - Updating files: - 正在更新文件： - MapETA @@ -682,18 +670,6 @@ location set Waiting for internet 等待网络连接 - - Choose Software to Install - 选择要安装的软件 - - - Dashcam - Dashcam（行车记录仪） - - - Custom Software - 自定义软件 - Enter URL 输入网址 @@ -1015,14 +991,6 @@ location set On this car, openpilot defaults to the car's built-in ACC instead of openpilot's longitudinal control. Enable this to switch to openpilot longitudinal control. Enabling Experimental mode is recommended when using experimental openpilot longitudinal control. 针对此车辆，openpilot默认使用车辆自带的ACC，而非openpilot的纵向控制。启用此选项将切换到openpilot纵向控制。当使用试验性的openpilot纵向控制时，建议同时启用试验模式。 - - Experimental mode is currently unavailable on this car, since the car's stock ACC is used for longitudinal control. - 由于此车辆使用自带的ACC纵向控制，当前无法使用试验模式。 - - - Enable experimental longitudinal control to allow experimental mode. - 启用试验性的纵向控制，以便允许使用试验模式。 - openpilot defaults to driving in <b>chill mode</b>. Experimental mode enables <b>alpha-level features</b> that aren't ready for chill mode. Experimental features are listed below: openpilot 默认 <b>轻松模式</b>驾驶车辆。试验模式启用一些轻松模式之外的 <b>试验性功能</b>。试验性功能包括： @@ -1043,6 +1011,22 @@ location set The driving visualization will transition to the road-facing wide-angle camera at low speeds to better show some turns. The Experimental mode logo will also be shown in the top right corner. 当低速行驶时，驾驶视角将切换到前向广角摄像头，便于更完整地显示转向路径。右上角将显示试验模式图标。 + + Experimental mode is currently unavailable on this car since the car's stock ACC is used for longitudinal control. + 由于此车辆使用自带的ACC纵向控制，当前无法使用试验模式。 + + + openpilot longitudinal control may come in a future update. + + + + An experimental version of openpilot longitudinal control can be tested, along with Experimental mode, on non-release branches. + + + + Enable experimental longitudinal control to allow Experimental mode. + 启用试验性的纵向控制，以便允许使用试验模式。 + Updater diff --git a/selfdrive/ui/translations/main_zh-CHT.ts b/selfdrive/ui/translations/main_zh-CHT.ts index 0379e926c4..71315e118f 100644 --- a/selfdrive/ui/translations/main_zh-CHT.ts +++ b/selfdrive/ui/translations/main_zh-CHT.ts @@ -311,18 +311,6 @@ Installing... 安裝中… - - Receiving objects: - 接收對象： - - - Resolving deltas: - 分析差異： - - - Updating files: - 更新檔案： - MapETA @@ -684,18 +672,6 @@ location set Waiting for internet 連接至網路中 - - Choose Software to Install - 選擇要安裝的軟體 - - - Dashcam - 行車記錄器 - - - Custom Software - 定制的軟體 - Enter URL 輸入網址 @@ -1017,14 +993,6 @@ location set On this car, openpilot defaults to the car's built-in ACC instead of openpilot's longitudinal control. Enable this to switch to openpilot longitudinal control. Enabling Experimental mode is recommended when using experimental openpilot longitudinal control. 在本車輛中，openpilot預設將使用原車內建的ACC系統，而非openpilot縱向控制。開啟此開關來啟用openpilot縱向控制，使用此選項時建議一併啟用實驗模式。 - - Experimental mode is currently unavailable on this car, since the car's stock ACC is used for longitudinal control. - 因車輛使用內建ACC系統，無法在本車輛上啟動實驗模式。 - - - Enable experimental longitudinal control to allow experimental mode. - 啟用實驗性縱向控制以使用實驗模式。 - openpilot defaults to driving in <b>chill mode</b>. Experimental mode enables <b>alpha-level features</b> that aren't ready for chill mode. Experimental features are listed below: openpilot 預設以 <b>輕鬆模式</b> 駕駛。 實驗模式啟用了尚未準備好進入輕鬆模式的 <b>alpha 級功能</b>。實驗功能如下： @@ -1045,6 +1013,22 @@ location set The driving visualization will transition to the road-facing wide-angle camera at low speeds to better show some turns. The Experimental mode logo will also be shown in the top right corner. 低速行駛時，將會切換成路側廣角鏡頭，以完整顯示轉彎路徑，右上角將出現實驗模式圖案。 + + Experimental mode is currently unavailable on this car since the car's stock ACC is used for longitudinal control. + 因車輛使用內建ACC系統，無法在本車輛上啟動實驗模式。 + + + openpilot longitudinal control may come in a future update. + + + + An experimental version of openpilot longitudinal control can be tested, along with Experimental mode, on non-release branches. + + + + Enable experimental longitudinal control to allow Experimental mode. + 啟用實驗性縱向控制以使用實驗模式。 + Updater diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index c62d737481..6c850b8ca4 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -112,6 +112,39 @@ void update_model(UIState *s, const cereal::ModelDataV2::Reader &model) { update_line_data(s, model_position, 0.9, 1.22, &scene.track_vertices, max_idx, false); } +void update_dmonitoring(UIState *s, const cereal::DriverStateV2::Reader &driverstate, float dm_fade_state, bool is_rhd) { + UIScene &scene = s->scene; + const auto driver_orient = is_rhd ? driverstate.getRightDriverData().getFaceOrientation() : driverstate.getLeftDriverData().getFaceOrientation(); + for (int i = 0; i < std::size(scene.driver_pose_vals); i++) { + float v_this = (i == 0 ? (driver_orient[i] < 0 ? 0.7 : 0.9) : 0.4) * driver_orient[i]; + scene.driver_pose_diff[i] = fabs(scene.driver_pose_vals[i] - v_this); + scene.driver_pose_vals[i] = 0.8 * v_this + (1 - 0.8) * scene.driver_pose_vals[i]; + scene.driver_pose_sins[i] = sinf(scene.driver_pose_vals[i]*(1.0-dm_fade_state)); + scene.driver_pose_coss[i] = cosf(scene.driver_pose_vals[i]*(1.0-dm_fade_state)); + } + + const mat3 r_xyz = (mat3){{ + scene.driver_pose_coss[1]*scene.driver_pose_coss[2], + scene.driver_pose_coss[1]*scene.driver_pose_sins[2], + -scene.driver_pose_sins[1], + + -scene.driver_pose_sins[0]*scene.driver_pose_sins[1]*scene.driver_pose_coss[2] - scene.driver_pose_coss[0]*scene.driver_pose_sins[2], + -scene.driver_pose_sins[0]*scene.driver_pose_sins[1]*scene.driver_pose_sins[2] + scene.driver_pose_coss[0]*scene.driver_pose_coss[2], + -scene.driver_pose_sins[0]*scene.driver_pose_coss[1], + + scene.driver_pose_coss[0]*scene.driver_pose_sins[1]*scene.driver_pose_coss[2] - scene.driver_pose_sins[0]*scene.driver_pose_sins[2], + scene.driver_pose_coss[0]*scene.driver_pose_sins[1]*scene.driver_pose_sins[2] + scene.driver_pose_sins[0]*scene.driver_pose_coss[2], + scene.driver_pose_coss[0]*scene.driver_pose_coss[1], + }}; + + // transform vertices + for (int kpi = 0; kpi < std::size(default_face_kpts_3d); kpi++) { + vec3 kpt_this = default_face_kpts_3d[kpi]; + kpt_this = matvecmul3(r_xyz, kpt_this); + scene.face_kpts_draw[kpi] = (vec3){{(float)kpt_this.v[0], (float)kpt_this.v[1], (float)(kpt_this.v[2] * (1.0-dm_fade_state) + 8 * dm_fade_state)}}; + } +} + static void update_sockets(UIState *s) { s->sm->update(0); } @@ -163,7 +196,7 @@ static void update_state(UIState *s) { scene.longitudinal_control = sm["carParams"].getCarParams().getOpenpilotLongitudinalControl(); } if (sm.updated("wideRoadCameraState")) { - float scale = (sm["wideRoadCameraState"].getWideRoadCameraState().getSensor() == cereal::FrameData::ImageSensor::AR0321) ? 6.0f : 1.0f; + float scale = (sm["wideRoadCameraState"].getWideRoadCameraState().getSensor() == cereal::FrameData::ImageSensor::AR0231) ? 6.0f : 1.0f; scene.light_sensor = std::max(100.0f - scale * sm["wideRoadCameraState"].getWideRoadCameraState().getExposureValPercent(), 0.0f); } scene.started = sm["deviceState"].getDeviceState().getStarted() && scene.ignition; @@ -213,7 +246,7 @@ void UIState::updateStatus() { UIState::UIState(QObject *parent) : QObject(parent) { sm = std::make_unique>({ "modelV2", "controlsState", "liveCalibration", "radarState", "deviceState", "roadCameraState", - "pandaStates", "carParams", "driverMonitoringState", "carState", "liveLocationKalman", + "pandaStates", "carParams", "driverMonitoringState", "carState", "liveLocationKalman", "driverStateV2", "wideRoadCameraState", "managerState", "navInstruction", "navRoute", "gnssMeasurements", }); diff --git a/selfdrive/ui/ui.h b/selfdrive/ui/ui.h index 9e1c54948b..e3eb97a762 100644 --- a/selfdrive/ui/ui.h +++ b/selfdrive/ui/ui.h @@ -25,6 +25,16 @@ typedef cereal::CarControl::HUDControl::AudibleAlert AudibleAlert; const mat3 DEFAULT_CALIBRATION = {{ 0.0, 1.0, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0 }}; +const vec3 default_face_kpts_3d[] = { + {-5.98, -51.20, 8.00}, {-17.64, -49.14, 8.00}, {-23.81, -46.40, 8.00}, {-29.98, -40.91, 8.00}, {-32.04, -37.49, 8.00}, + {-34.10, -32.00, 8.00}, {-36.16, -21.03, 8.00}, {-36.16, 6.40, 8.00}, {-35.47, 10.51, 8.00}, {-32.73, 19.43, 8.00}, + {-29.30, 26.29, 8.00}, {-24.50, 33.83, 8.00}, {-19.01, 41.37, 8.00}, {-14.21, 46.17, 8.00}, {-12.16, 47.54, 8.00}, + {-4.61, 49.60, 8.00}, {4.99, 49.60, 8.00}, {12.53, 47.54, 8.00}, {14.59, 46.17, 8.00}, {19.39, 41.37, 8.00}, + {24.87, 33.83, 8.00}, {29.67, 26.29, 8.00}, {33.10, 19.43, 8.00}, {35.84, 10.51, 8.00}, {36.53, 6.40, 8.00}, + {36.53, -21.03, 8.00}, {34.47, -32.00, 8.00}, {32.42, -37.49, 8.00}, {30.36, -40.91, 8.00}, {24.19, -46.40, 8.00}, + {18.02, -49.14, 8.00}, {6.36, -51.20, 8.00}, {-5.98, -51.20, 8.00}, +}; + struct Alert { QString text1; QString text2; @@ -103,6 +113,13 @@ typedef struct UIScene { // lead QPointF lead_vertices[2]; + // DMoji state + float driver_pose_vals[3]; + float driver_pose_diff[3]; + float driver_pose_sins[3]; + float driver_pose_coss[3]; + vec3 face_kpts_draw[std::size(default_face_kpts_3d)]; + float light_sensor; bool started, ignition, is_metric, map_on_left, longitudinal_control; uint64_t started_frame; @@ -183,6 +200,7 @@ public slots: void ui_update_params(UIState *s); int get_path_length_idx(const cereal::ModelDataV2::XYZTData::Reader &line, const float path_height); void update_model(UIState *s, const cereal::ModelDataV2::Reader &model); +void update_dmonitoring(UIState *s, const cereal::DriverStateV2::Reader &driverstate, float dm_fade_state, bool is_rhd); void update_leads(UIState *s, const cereal::RadarState::Reader &radar_state, const cereal::ModelDataV2::XYZTData::Reader &line); void update_line_data(const UIState *s, const cereal::ModelDataV2::XYZTData::Reader &line, float y_off, float z_off, QPolygonF *pvd, int max_idx, bool allow_invert); diff --git a/selfdrive/updated.py b/selfdrive/updated.py index 9da2a05a11..2cb7d1c13b 100755 --- a/selfdrive/updated.py +++ b/selfdrive/updated.py @@ -423,6 +423,9 @@ def main() -> None: wait_helper = WaitTimeHelper() wait_helper.only_check_for_update = True + # invalidate old finalized update + set_consistent_flag(False) + # Run the update loop while True: wait_helper.ready_event.clear() diff --git a/system/camerad/cameras/camera_common.cc b/system/camerad/cameras/camera_common.cc index 30e2810ec4..7ee3738057 100644 --- a/system/camerad/cameras/camera_common.cc +++ b/system/camerad/cameras/camera_common.cc @@ -167,7 +167,7 @@ void fill_frame_data(cereal::FrameData::Builder &framed, const FrameMetadata &fr framed.setExposureValPercent(perc); if (c->camera_id == CAMERA_ID_AR0231) { - framed.setSensor(cereal::FrameData::ImageSensor::AR0321); + framed.setSensor(cereal::FrameData::ImageSensor::AR0231); } else if (c->camera_id == CAMERA_ID_OX03C10) { framed.setSensor(cereal::FrameData::ImageSensor::OX03C10); } diff --git a/system/camerad/cameras/camera_qcom2.cc b/system/camerad/cameras/camera_qcom2.cc index 091b0d91d9..92b3bde413 100644 --- a/system/camerad/cameras/camera_qcom2.cc +++ b/system/camerad/cameras/camera_qcom2.cc @@ -60,15 +60,15 @@ CameraInfo cameras_supported[CAMERA_ID_MAX] = { const float DC_GAIN_AR0231 = 2.5; const float DC_GAIN_OX03C10 = 7.32; -const float DC_GAIN_ON_GREY_AR0231= 0.2; +const float DC_GAIN_ON_GREY_AR0231 = 0.2; const float DC_GAIN_OFF_GREY_AR0231 = 0.3; -const float DC_GAIN_ON_GREY_OX03C10= 0.25; -const float DC_GAIN_OFF_GREY_OX03C10 = 0.35; +const float DC_GAIN_ON_GREY_OX03C10 = 0.9; +const float DC_GAIN_OFF_GREY_OX03C10 = 1.0; const int DC_GAIN_MIN_WEIGHT_AR0231 = 0; const int DC_GAIN_MAX_WEIGHT_AR0231 = 1; -const int DC_GAIN_MIN_WEIGHT_OX03C10 = 16; -const int DC_GAIN_MAX_WEIGHT_OX03C10 = 32; +const int DC_GAIN_MIN_WEIGHT_OX03C10 = 1; // always on is fine +const int DC_GAIN_MAX_WEIGHT_OX03C10 = 1; const float TARGET_GREY_FACTOR_AR0231 = 1.0; const float TARGET_GREY_FACTOR_OX03C10 = 0.02; @@ -104,8 +104,8 @@ const int ANALOG_GAIN_MIN_IDX_OX03C10 = 0x0; const int ANALOG_GAIN_REC_IDX_OX03C10 = 0x11; // 2.5x const int ANALOG_GAIN_MAX_IDX_OX03C10 = 0x36; const int ANALOG_GAIN_COST_DELTA_OX03C10 = -1; -const float ANALOG_GAIN_COST_LOW_OX03C10 = 0.05; -const float ANALOG_GAIN_COST_HIGH_OX03C10 = 0.8; +const float ANALOG_GAIN_COST_LOW_OX03C10 = 0.4; +const float ANALOG_GAIN_COST_HIGH_OX03C10 = 6.4; const int EXPOSURE_TIME_MIN_AR0231 = 2; // with HDR, fastest ss const int EXPOSURE_TIME_MAX_AR0231 = 0x0855; // with HDR, slowest ss, 40ms @@ -1041,6 +1041,30 @@ void CameraState::handle_camera_event(void *evdat) { } } +void CameraState::update_exposure_score(float desired_ev, int exp_t, int exp_g_idx, float exp_gain) { + float score = 1e6; + if (camera_id == CAMERA_ID_AR0231) { + // Cost of ev diff + score = std::abs(desired_ev - (exp_t * exp_gain)) * 10; + // Cost of absolute gain + float m = exp_g_idx > analog_gain_rec_idx ? analog_gain_cost_high : analog_gain_cost_low; + score += std::abs(exp_g_idx - (int)analog_gain_rec_idx) * m; + // Cost of changing gain + score += std::abs(exp_g_idx - gain_idx) * (score + 1.0) / 10.0; + } else if (camera_id == CAMERA_ID_OX03C10) { + score = std::abs(desired_ev - (exp_t * exp_gain)); + float m = exp_g_idx > analog_gain_rec_idx ? analog_gain_cost_high : analog_gain_cost_low; + score += std::abs(exp_g_idx - (int)analog_gain_rec_idx) * m; + score += ((1 - analog_gain_cost_delta) + analog_gain_cost_delta * (exp_g_idx - analog_gain_min_idx) / (analog_gain_max_idx - analog_gain_min_idx)) * std::abs(exp_g_idx - gain_idx) * 5.0; + } + + if (score < best_ev_score) { + new_exp_t = exp_t; + new_exp_g = exp_g_idx; + best_ev_score = score; + } +} + void CameraState::set_camera_exposure(float grey_frac) { if (!enabled) return; const float dt = 0.05; @@ -1066,9 +1090,9 @@ void CameraState::set_camera_exposure(float grey_frac) { float k = (1.0 - k_ev) / 3.0; desired_ev = (k * cur_ev[0]) + (k * cur_ev[1]) + (k * cur_ev[2]) + (k_ev * desired_ev); - float best_ev_score = 1e6; - int new_g = 0; - int new_t = 0; + best_ev_score = 1e6; + new_exp_g = 0; + new_exp_t = 0; // Hysteresis around high conversion gain // We usually want this on since it results in lower noise, but turn off in very bright day scenes @@ -1095,8 +1119,8 @@ void CameraState::set_camera_exposure(float grey_frac) { gain_idx = std::stoi(gain_bytes); exposure_time = std::stoi(time_bytes); - new_g = gain_idx; - new_t = exposure_time; + new_exp_g = gain_idx; + new_exp_t = exposure_time; enable_dc_gain = false; } else { // Simple brute force optimizer to choose sensor parameters @@ -1112,23 +1136,7 @@ void CameraState::set_camera_exposure(float grey_frac) { continue; } - // Compute error to desired ev - float score = std::abs(desired_ev - (t * gain)) * 10; - - // Going below recommended gain needs lower penalty to not overexpose - float m = g > analog_gain_rec_idx ? analog_gain_cost_high : analog_gain_cost_low; - score += std::abs(g - (int)analog_gain_rec_idx) * m; - - // LOGE("cam: %d - gain: %d, t: %d (%.2f), score %.2f, score + gain %.2f, %.3f, %.3f", camera_num, g, t, desired_ev / gain, score, score + std::abs(g - gain_idx) * (score + 1.0) / 10.0, desired_ev, min_ev); - - // Small penalty on changing gain - score += ((1 - analog_gain_cost_delta) + analog_gain_cost_delta * (g - analog_gain_min_idx) / (analog_gain_max_idx - analog_gain_min_idx)) * std::abs(g - gain_idx) * (score + 1.0) / 10.0; - - if (score < best_ev_score) { - new_t = t; - new_g = g; - best_ev_score = score; - } + update_exposure_score(desired_ev, t, g, gain); } } @@ -1137,9 +1145,9 @@ void CameraState::set_camera_exposure(float grey_frac) { measured_grey_fraction = grey_frac; target_grey_fraction = target_grey; - analog_gain_frac = sensor_analog_gains[new_g]; - gain_idx = new_g; - exposure_time = new_t; + analog_gain_frac = sensor_analog_gains[new_exp_g]; + gain_idx = new_exp_g; + exposure_time = new_exp_t; dc_gain_enabled = enable_dc_gain; float gain = analog_gain_frac * (1 + dc_gain_weight * (dc_gain_factor-1) / dc_gain_max_weight); @@ -1156,7 +1164,7 @@ void CameraState::set_camera_exposure(float grey_frac) { // LOGE("ae - camera %d, cur_t %.5f, sof %.5f, dt %.5f", camera_num, 1e-9 * nanos_since_boot(), 1e-9 * buf.cur_frame_data.timestamp_sof, 1e-9 * (nanos_since_boot() - buf.cur_frame_data.timestamp_sof)); if (camera_id == CAMERA_ID_AR0231) { - uint16_t analog_gain_reg = 0xFF00 | (new_g << 4) | new_g; + uint16_t analog_gain_reg = 0xFF00 | (new_exp_g << 4) | new_exp_g; struct i2c_random_wr_payload exp_reg_array[] = { {0x3366, analog_gain_reg}, {0x3362, (uint16_t)(dc_gain_enabled ? 0x1 : 0x0)}, @@ -1164,15 +1172,16 @@ void CameraState::set_camera_exposure(float grey_frac) { }; sensors_i2c(exp_reg_array, sizeof(exp_reg_array)/sizeof(struct i2c_random_wr_payload), CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG, true); } else if (camera_id == CAMERA_ID_OX03C10) { - // t_HCG + t_LCG + t_VS on LPD, t_SPD on SPD - uint32_t hcg_time = std::max((dc_gain_weight * exposure_time / dc_gain_max_weight), 0); - uint32_t lcg_time = std::max(((dc_gain_max_weight - dc_gain_weight) * exposure_time / dc_gain_max_weight), 0); - // uint32_t spd_time = std::max(hcg_time / 16, (uint32_t)exposure_time_min); - uint32_t vs_time = std::min(std::max((uint32_t)exposure_time / 128, VS_TIME_MIN_OX03C10), VS_TIME_MAX_OX03C10); - uint32_t spd_time = vs_time; - - uint32_t real_gain = ox03c10_analog_gains_reg[new_g]; + // t_HCG&t_LCG + t_VS on LPD, t_SPD on SPD + uint32_t hcg_time = exposure_time; + uint32_t lcg_time = hcg_time; + uint32_t spd_time = exposure_time_max + VS_TIME_MAX_OX03C10; + uint32_t vs_time = std::min(std::max((uint32_t)exposure_time / 40, VS_TIME_MIN_OX03C10), VS_TIME_MAX_OX03C10); + + uint32_t real_gain = ox03c10_analog_gains_reg[new_exp_g]; uint32_t min_gain = ox03c10_analog_gains_reg[0]; + uint32_t spd_gain = 0xF00; + struct i2c_random_wr_payload exp_reg_array[] = { {0x3501, hcg_time>>8}, {0x3502, hcg_time&0xFF}, {0x3581, lcg_time>>8}, {0x3582, lcg_time&0xFF}, @@ -1181,7 +1190,7 @@ void CameraState::set_camera_exposure(float grey_frac) { {0x3508, real_gain>>8}, {0x3509, real_gain&0xFF}, {0x3588, min_gain>>8}, {0x3589, min_gain&0xFF}, - {0x3548, min_gain>>8}, {0x3549, min_gain&0xFF}, + {0x3548, spd_gain>>8}, {0x3549, spd_gain&0xFF}, {0x35c8, min_gain>>8}, {0x35c9, min_gain&0xFF}, }; sensors_i2c(exp_reg_array, sizeof(exp_reg_array)/sizeof(struct i2c_random_wr_payload), CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG, false); diff --git a/system/camerad/cameras/camera_qcom2.h b/system/camerad/cameras/camera_qcom2.h index 9f0c3743f1..9e0109ab20 100644 --- a/system/camerad/cameras/camera_qcom2.h +++ b/system/camerad/cameras/camera_qcom2.h @@ -47,6 +47,9 @@ public: float cur_ev[3]; float min_ev, max_ev; + float best_ev_score; + int new_exp_g; + int new_exp_t; float measured_grey_fraction; float target_grey_fraction; @@ -58,6 +61,7 @@ public: int camera_num; void handle_camera_event(void *evdat); + void update_exposure_score(float desired_ev, int exp_t, int exp_g_idx, float exp_gain); void set_camera_exposure(float grey_frac); void sensors_start(); diff --git a/system/camerad/cameras/sensor2_i2c.h b/system/camerad/cameras/sensor2_i2c.h index ab51059d9a..83fcb8f7a9 100644 --- a/system/camerad/cameras/sensor2_i2c.h +++ b/system/camerad/cameras/sensor2_i2c.h @@ -126,7 +126,7 @@ struct i2c_random_wr_payload init_array_ox03c10[] = { {0x3219, 0x08}, {0x3506, 0x20}, {0x3507, 0x00}, // hcg fine exposure - {0x350a, 0x04}, {0x350b, 0x00}, {0x350c, 0x00}, // hcg digital gain + {0x350a, 0x01}, {0x350b, 0x00}, {0x350c, 0x00}, // hcg digital gain {0x3586, 0x40}, {0x3587, 0x00}, // lcg fine exposure {0x358a, 0x01}, {0x358b, 0x00}, {0x358c, 0x00}, // lcg digital gain @@ -711,11 +711,11 @@ struct i2c_random_wr_payload init_array_ox03c10[] = { {0x4221, 0x03}, // this is changed from 1 -> 3 // DCG exposure coarse - {0x3501, 0x01}, {0x3502, 0xc8}, + // {0x3501, 0x01}, {0x3502, 0xc8}, // SPD exposure coarse - {0x3541, 0x01}, {0x3542, 0xc8}, + // {0x3541, 0x01}, {0x3542, 0xc8}, // VS exposure coarse - {0x35c1, 0x00}, {0x35c2, 0x01}, + // {0x35c1, 0x00}, {0x35c2, 0x01}, // crc reference {0x420e, 0x66}, {0x420f, 0x5d}, {0x4210, 0xa8}, {0x4211, 0x55}, diff --git a/system/camerad/test/test_camerad.py b/system/camerad/test/test_camerad.py index 6c2ef1c7bc..f03c531b20 100755 --- a/system/camerad/test/test_camerad.py +++ b/system/camerad/test/test_camerad.py @@ -1,6 +1,7 @@ #!/usr/bin/env python3 import time import unittest +import numpy as np from collections import defaultdict import cereal.messaging as messaging @@ -10,8 +11,10 @@ from selfdrive.manager.process_config import managed_processes from system.hardware import TICI TEST_TIMESPAN = 30 -LAG_FRAME_TOLERANCE = {log.FrameData.ImageSensor.ar0321: 0.5, # ARs use synced pulses for frame starts +LAG_FRAME_TOLERANCE = {log.FrameData.ImageSensor.ar0231: 0.5, # ARs use synced pulses for frame starts log.FrameData.ImageSensor.ox03c10: 1.0} # OXs react to out-of-sync at next frame +FRAME_DELTA_TOLERANCE = {log.FrameData.ImageSensor.ar0231: 1.0, + log.FrameData.ImageSensor.ox03c10: 1.0} CAMERAS = ('roadCameraState', 'driverCameraState', 'wideRoadCameraState') @@ -36,10 +39,16 @@ class TestCamerad(unittest.TestCase): managed_processes['camerad'].stop() cls.log_by_frame_id = defaultdict(list) + cls.sensor_type = None for cam, msgs in cls.logs.items(): + if cls.sensor_type is None: + cls.sensor_type = getattr(msgs[0], msgs[0].which()).sensor.raw expected_frames = service_list[cam].frequency * TEST_TIMESPAN assert expected_frames*0.95 < len(msgs) < expected_frames*1.05, f"unexpected frame count {cam}: {expected_frames=}, got {len(msgs)}" + dts = np.abs(np.diff([getattr(m, m.which()).timestampSof/1e6 for m in msgs]) - 1000/service_list[cam].frequency) + assert (dts < FRAME_DELTA_TOLERANCE[cls.sensor_type]).all(), f"{cam} dts(ms) out of spec: max diff {dts.max()}, 99 percentile {np.percentile(dts, 99)}" + for m in msgs: cls.log_by_frame_id[getattr(m, m.which()).frameId].append(m) @@ -64,17 +73,16 @@ class TestCamerad(unittest.TestCase): assert len(skips) == 0, f"Found frame skips, missing cameras for the following frames: {skips}" def test_frame_sync(self): - sensor_type = [getattr(msgs[0], msgs[0].which()).sensor for frame_id, msgs in self.log_by_frame_id.items()][0].raw frame_times = {frame_id: [getattr(m, m.which()).timestampSof for m in msgs] for frame_id, msgs in self.log_by_frame_id.items()} diffs = {frame_id: (max(ts) - min(ts))/1e6 for frame_id, ts in frame_times.items()} def get_desc(fid, diff): cam_times = [(m.which(), getattr(m, m.which()).timestampSof/1e6) for m in self.log_by_frame_id[fid]] return (diff, cam_times) - laggy_frames = {k: get_desc(k, v) for k, v in diffs.items() if v > LAG_FRAME_TOLERANCE[sensor_type]} + laggy_frames = {k: get_desc(k, v) for k, v in diffs.items() if v > LAG_FRAME_TOLERANCE[self.sensor_type]} def in_tol(diff): - return 50 - LAG_FRAME_TOLERANCE[sensor_type] < diff and diff < 50 + LAG_FRAME_TOLERANCE[sensor_type] + return 50 - LAG_FRAME_TOLERANCE[self.sensor_type] < diff and diff < 50 + LAG_FRAME_TOLERANCE[self.sensor_type] if len(laggy_frames) != 0 and all( in_tol(laggy_frames[lf][0]) for lf in laggy_frames): print("TODO: handle camera out of sync") else: diff --git a/system/camerad/test/test_exposure.py b/system/camerad/test/test_exposure.py index 8cce7e7ffa..201b205c4f 100755 --- a/system/camerad/test/test_exposure.py +++ b/system/camerad/test/test_exposure.py @@ -6,16 +6,13 @@ import numpy as np from selfdrive.test.helpers import with_processes from system.camerad.snapshot.snapshot import get_snapshots -from system.hardware import TICI - TEST_TIME = 45 REPEAT = 5 class TestCamerad(unittest.TestCase): @classmethod def setUpClass(cls): - if not TICI: - raise unittest.SkipTest + pass def _numpy_rgb2gray(self, im): ret = np.clip(im[:,:,2] * 0.114 + im[:,:,1] * 0.587 + im[:,:,0] * 0.299, 0, 255).astype(np.uint8) @@ -37,13 +34,11 @@ class TestCamerad(unittest.TestCase): start = time.time() while time.time() - start < TEST_TIME and passed < REPEAT: rpic, dpic = get_snapshots(frame="roadCameraState", front_frame="driverCameraState") + wpic, _ = get_snapshots(frame="wideRoadCameraState") res = self._is_exposure_okay(rpic) res = res and self._is_exposure_okay(dpic) - - if TICI: - wpic, _ = get_snapshots(frame="wideRoadCameraState") - res = res and self._is_exposure_okay(wpic) + res = res and self._is_exposure_okay(wpic) if passed > 0 and not res: passed = -passed # fails test if any failure after first sus diff --git a/system/hardware/base.h b/system/hardware/base.h index f6e0b42d73..6cfc1d8743 100644 --- a/system/hardware/base.h +++ b/system/hardware/base.h @@ -16,6 +16,8 @@ public: static int get_voltage() { return 0; }; static int get_current() { return 0; }; + static std::string get_serial() { return "cccccc"; } + static void reboot() {} static void poweroff() {} static void set_brightness(int percent) {} diff --git a/system/hardware/tici/hardware.h b/system/hardware/tici/hardware.h index 521a257627..5f6fb2dc50 100644 --- a/system/hardware/tici/hardware.h +++ b/system/hardware/tici/hardware.h @@ -21,6 +21,23 @@ public: static int get_voltage() { return std::atoi(util::read_file("/sys/class/hwmon/hwmon1/in1_input").c_str()); }; static int get_current() { return std::atoi(util::read_file("/sys/class/hwmon/hwmon1/curr1_input").c_str()); }; + static std::string get_serial() { + static std::string serial(""); + if (serial.empty()) { + std::ifstream stream("/proc/cmdline"); + std::string cmdline; + std::getline(stream, cmdline); + + auto start = cmdline.find("serialno="); + if (start == std::string::npos) { + serial = "cccccc"; + } else { + auto end = cmdline.find(" ", start + 9); + serial = cmdline.substr(start + 9, end - start - 9); + } + } + return serial; + } static void reboot() { std::system("sudo reboot"); }; static void poweroff() { std::system("sudo poweroff"); }; diff --git a/system/micd.py b/system/micd.py index a56140e3b9..97ba0c262e 100755 --- a/system/micd.py +++ b/system/micd.py @@ -1,5 +1,4 @@ #!/usr/bin/env python3 -import sounddevice as sd import numpy as np from cereal import messaging @@ -84,11 +83,11 @@ class Mic: self.measurements = self.measurements[FFT_SAMPLES:] - def micd_thread(self, device=None): - if device is None: - device = "sysdefault" + def micd_thread(self): + # sounddevice must be imported after forking processes + import sounddevice as sd # pylint: disable=import-outside-toplevel - with sd.InputStream(device=device, channels=1, samplerate=SAMPLE_RATE, callback=self.callback) as stream: + with sd.InputStream(channels=1, samplerate=SAMPLE_RATE, callback=self.callback) as stream: cloudlog.info(f"micd stream started: {stream.samplerate=} {stream.channels=} {stream.dtype=} {stream.device=}") while True: self.update() diff --git a/system/version.py b/system/version.py index 6031531556..55f80afa35 100644 --- a/system/version.py +++ b/system/version.py @@ -7,7 +7,7 @@ from functools import lru_cache from common.basedir import BASEDIR from system.swaglog import cloudlog -RELEASE_BRANCHES = ['release3-staging', 'dashcam3-staging', 'release3', 'dashcam3'] +RELEASE_BRANCHES = ['release3-staging', 'dashcam3-staging', 'release3', 'dashcam3', 'nightly'] TESTED_BRANCHES = RELEASE_BRANCHES + ['devel', 'devel-staging'] training_version: bytes = b"0.2.0" diff --git a/third_party/bootstrap/.gitignore b/third_party/bootstrap/.gitignore new file mode 100644 index 0000000000..ac06c0cf85 --- /dev/null +++ b/third_party/bootstrap/.gitignore @@ -0,0 +1 @@ +/icons/ diff --git a/third_party/bootstrap/bootstrap-icons.svg b/third_party/bootstrap/bootstrap-icons.svg new file mode 100644 index 0000000000..61f2720db4 --- /dev/null +++ b/third_party/bootstrap/bootstrap-icons.svg @@ -0,0 +1 @@ + \ No newline at end of file diff --git a/third_party/bootstrap/pull.sh b/third_party/bootstrap/pull.sh new file mode 100755 index 0000000000..0b03b4db9e --- /dev/null +++ b/third_party/bootstrap/pull.sh @@ -0,0 +1,14 @@ +#!/bin/bash +set -e + +DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null && pwd )" +cd $DIR + +if [ ! -d icons/ ]; then + git clone https://github.com/twbs/icons/ +fi + +cd icons +git fetch --all +git checkout d5aa187483a1b0b186f87adcfa8576350d970d98 +cp bootstrap-icons.svg ../ diff --git a/tinygrad_repo b/tinygrad_repo index 8e22d5ee67..2e1d47b166 160000 --- a/tinygrad_repo +++ b/tinygrad_repo @@ -1 +1 @@ -Subproject commit 8e22d5ee675277181e1eff644dde9e844fc40fce +Subproject commit 2e1d47b16625ff343516287cdd9e4bcb26f5c4ef diff --git a/tools/cabana/README.md b/tools/cabana/README.md index dd131880a6..db247c39c5 100644 --- a/tools/cabana/README.md +++ b/tools/cabana/README.md @@ -8,13 +8,18 @@ Cabana is a tool developed to view raw CAN data. One use for this is creating an ```bash $ ./cabana -h -Usage: ./cabana [options] route +Usage: ./_cabana [options] route Options: -h, --help Displays this help. --demo use a demo route instead of providing your own --qcam load qcamera + --ecam load wide road camera + --stream read can messages from live streaming + --zmq the ip address on which to receive zmq messages --data_dir local directory with routes + --no-vipc do not output video + --dbc dbc file to open Arguments: route the drive to replay. find your drives at diff --git a/tools/cabana/SConscript b/tools/cabana/SConscript index 718e2e50af..a9922ba9be 100644 --- a/tools/cabana/SConscript +++ b/tools/cabana/SConscript @@ -1,6 +1,6 @@ import os Import('env', 'qt_env', 'arch', 'common', 'messaging', 'visionipc', 'replay_lib', - 'cereal', 'transformations', 'widgets', 'opendbc') + 'cereal', 'transformations', 'widgets', 'opendbc', 'asset_obj') base_frameworks = qt_env['FRAMEWORKS'] base_libs = [common, messaging, cereal, visionipc, transformations, 'zmq', @@ -8,18 +8,24 @@ base_libs = [common, messaging, cereal, visionipc, transformations, 'zmq', if arch == "Darwin": base_frameworks.append('OpenCL') + base_frameworks.append('QtCharts') else: base_libs.append('OpenCL') + base_libs.append('Qt5Charts') + +qt_libs = ['qt_util'] + base_libs -qt_libs = ['qt_util', 'Qt5Charts'] + base_libs cabana_libs = [widgets, cereal, messaging, visionipc, replay_lib, opendbc,'avutil', 'avcodec', 'avformat', 'bz2', 'curl', 'yuv'] + qt_libs cabana_env = qt_env.Clone() prev_moc_path = cabana_env['QT_MOCHPREFIX'] cabana_env['QT_MOCHPREFIX'] = os.path.dirname(prev_moc_path) + '/cabana/moc_' -cabana_lib = cabana_env.Library("cabana_lib", ['mainwin.cc', 'binaryview.cc', 'chartswidget.cc', 'historylog.cc', 'videowidget.cc', 'signaledit.cc', 'dbcmanager.cc', - 'canmessages.cc', 'commands.cc', 'messageswidget.cc', 'settings.cc', 'detailwidget.cc', 'tools/findsimilarbits.cc'], LIBS=cabana_libs, FRAMEWORKS=base_frameworks) -cabana_env.Program('_cabana', ['cabana.cc', cabana_lib], LIBS=cabana_libs, FRAMEWORKS=base_frameworks) +cabana_lib = cabana_env.Library("cabana_lib", ['mainwin.cc', 'streams/livestream.cc', 'streams/abstractstream.cc', 'streams/replaystream.cc', 'binaryview.cc', 'chartswidget.cc', 'historylog.cc', 'videowidget.cc', 'signaledit.cc', 'dbcmanager.cc', + 'commands.cc', 'messageswidget.cc', 'route.cc', 'settings.cc', 'util.cc', 'detailwidget.cc', 'tools/findsimilarbits.cc'], LIBS=cabana_libs, FRAMEWORKS=base_frameworks) +cabana_env.Program('_cabana', ['cabana.cc', cabana_lib, asset_obj], LIBS=cabana_libs, FRAMEWORKS=base_frameworks) + +if arch == "Darwin": + cabana_env.Execute('install_name_tool -change opendbc/can/libdbc.dylib @loader_path/../../opendbc/can/libdbc.dylib ./_cabana') if GetOption('test'): cabana_env.Program('tests/_test_cabana', ['tests/test_runner.cc', 'tests/test_cabana.cc', cabana_lib], LIBS=[cabana_libs]) diff --git a/tools/cabana/binaryview.cc b/tools/cabana/binaryview.cc index b98c75d452..52b31b3559 100644 --- a/tools/cabana/binaryview.cc +++ b/tools/cabana/binaryview.cc @@ -4,13 +4,18 @@ #include #include #include +#include #include -#include "tools/cabana/canmessages.h" +#include + +#include "tools/cabana/commands.h" +#include "tools/cabana/streams/abstractstream.h" // BinaryView const int CELL_HEIGHT = 36; +const int VERTICAL_HEADER_WIDTH = 30; inline int get_bit_index(const QModelIndex &index, bool little_endian) { return index.row() * 8 + (little_endian ? 7 - index.column() : index.column()); @@ -29,14 +34,27 @@ BinaryView::BinaryView(QWidget *parent) : QTableView(parent) { setFrameShape(QFrame::NoFrame); setShowGrid(false); setMouseTracking(true); - setSizeAdjustPolicy(QAbstractScrollArea::AdjustToContents); - setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Fixed); + setHorizontalScrollBarPolicy(Qt::ScrollBarAlwaysOff); + + QObject::connect(dbc(), &DBCManager::DBCFileChanged, this, &BinaryView::refresh); + QObject::connect(UndoStack::instance(), &QUndoStack::indexChanged, this, &BinaryView::refresh); +} + +QSize BinaryView::minimumSizeHint() const { + return {(horizontalHeader()->minimumSectionSize() + 1) * 9 + VERTICAL_HEADER_WIDTH, + CELL_HEIGHT * std::min(model->rowCount(), 10)}; } void BinaryView::highlight(const Signal *sig) { if (sig != hovered_sig) { + for (int i = 0; i < model->items.size(); ++i) { + auto &item_sigs = model->items[i].sigs; + if ((sig && item_sigs.contains(sig)) || (hovered_sig && item_sigs.contains(hovered_sig))) { + auto index = model->index(i / model->columnCount(), i % model->columnCount()); + emit model->dataChanged(index, index, {Qt::DisplayRole}); + } + } hovered_sig = sig; - model->dataChanged(model->index(0, 0), model->index(model->rowCount() - 1, model->columnCount() - 1)); emit signalHovered(hovered_sig); } } @@ -113,13 +131,20 @@ void BinaryView::leaveEvent(QEvent *event) { } void BinaryView::setMessage(const QString &message_id) { - model->setMessage(message_id); + model->msg_id = message_id; + verticalScrollBar()->setValue(0); + refresh(); +} + +void BinaryView::refresh() { + if (model->msg_id.isEmpty()) return; + clearSelection(); anchor_index = QModelIndex(); resize_sig = nullptr; hovered_sig = nullptr; + model->refresh(); highlightPosition(QCursor::pos()); - updateState(); } QSet BinaryView::getOverlappingSignals() const { @@ -144,14 +169,12 @@ std::tuple BinaryView::getSelection(QModelIndex index) { // BinaryViewModel -void BinaryViewModel::setMessage(const QString &message_id) { +void BinaryViewModel::refresh() { beginResetModel(); - msg_id = message_id; items.clear(); if ((dbc_msg = dbc()->msg(msg_id))) { row_count = dbc_msg->size; items.resize(row_count * column_count); - int i = 0; for (auto sig : dbc_msg->getSignals()) { auto [start, end] = getSignalRange(sig); for (int j = start; j <= end; ++j) { @@ -163,21 +186,23 @@ void BinaryViewModel::setMessage(const QString &message_id) { } if (j == start) sig->is_little_endian ? items[idx].is_lsb = true : items[idx].is_msb = true; if (j == end) sig->is_little_endian ? items[idx].is_msb = true : items[idx].is_lsb = true; - items[idx].bg_color = getColor(i); + items[idx].bg_color = getColor(sig); items[idx].sigs.push_back(sig); } - ++i; } } else { row_count = can->lastMessage(msg_id).dat.size(); items.resize(row_count * column_count); } endResetModel(); + updateState(); } void BinaryViewModel::updateState() { auto prev_items = items; - const auto &binary = can->lastMessage(msg_id).dat; + const auto &last_msg = can->lastMessage(msg_id); + const auto &binary = last_msg.dat; + // data size may changed. if (binary.size() > row_count) { beginInsertRows({}, row_count, binary.size() - 1); @@ -187,12 +212,26 @@ void BinaryViewModel::updateState() { } char hex[3] = {'\0'}; for (int i = 0; i < binary.size(); ++i) { - for (int j = 0; j < column_count - 1; ++j) { + for (int j = 0; j < 8; ++j) { items[i * column_count + j].val = ((binary[i] >> (7 - j)) & 1) != 0 ? '1' : '0'; + + // Bit update frequency based highlighting + bool has_signal = items[i * column_count + j].sigs.size() > 0; + double offset = has_signal ? 50 : 0; + + double min_f = last_msg.bit_change_counts[i][7 - j] == 0 ? offset : offset + 25; + double max_f = 255.0; + + double factor = 0.25; + double scaler = max_f / log2(1.0 + factor); + + double alpha = std::clamp(offset + log2(1.0 + factor * (double)last_msg.bit_change_counts[i][7 - j] / (double)last_msg.count) * scaler, min_f, max_f); + items[i * column_count + j].bg_color.setAlpha(alpha); } hex[0] = toHex(binary[i] >> 4); hex[1] = toHex(binary[i] & 0xf); items[i * column_count + 8].val = hex; + items[i * column_count + 8].bg_color = last_msg.colors[i]; } for (int i = binary.size(); i < row_count; ++i) { for (int j = 0; j < column_count; ++j) { @@ -200,8 +239,8 @@ void BinaryViewModel::updateState() { } } - for (int i = 0; i < row_count * column_count; ++i) { - if (i >= prev_items.size() || prev_items[i].val != items[i].val) { + for (int i = 0; i < items.size(); ++i) { + if (i >= prev_items.size() || prev_items[i].val != items[i].val || prev_items[i].bg_color != items[i].bg_color) { auto idx = index(i / column_count, i % column_count); emit dataChanged(idx, idx); } @@ -212,7 +251,7 @@ QVariant BinaryViewModel::headerData(int section, Qt::Orientation orientation, i if (orientation == Qt::Vertical) { switch (role) { case Qt::DisplayRole: return section; - case Qt::SizeHintRole: return QSize(30, 0); + case Qt::SizeHintRole: return QSize(VERTICAL_HEADER_WIDTH, 0); case Qt::TextAlignmentRole: return Qt::AlignCenter; } } @@ -234,14 +273,24 @@ void BinaryItemDelegate::paint(QPainter *painter, const QStyleOptionViewItem &op if (index.column() == 8) { painter->setFont(hex_font); + painter->fillRect(option.rect, item->bg_color); } else if (option.state & QStyle::State_Selected) { painter->fillRect(option.rect, selection_color); painter->setPen(option.palette.color(QPalette::BrightText)); } else if (!item->sigs.isEmpty() && (!bin_view->selectionModel()->hasSelection() || !item->sigs.contains(bin_view->resize_sig))) { + bool sig_hovered = item->sigs.contains(bin_view->hovered_sig); + int min_alpha = item->sigs.contains(bin_view->hovered_sig) ? 255 : 50; + QColor bg = item->bg_color; + if (bg.alpha() < min_alpha) { + bg.setAlpha(min_alpha); + } + painter->fillRect(option.rect, sig_hovered ? bg.darker(125) : bg); // 4/5x brightness + painter->setPen(sig_hovered ? option.palette.color(QPalette::BrightText) : Qt::black); + } else { painter->fillRect(option.rect, item->bg_color); - painter->setPen(item->sigs.contains(bin_view->hovered_sig) ? option.palette.color(QPalette::BrightText) : Qt::black); } + painter->drawText(option.rect, Qt::AlignCenter, item->val); if (item->is_msb || item->is_lsb) { painter->setFont(small_font); diff --git a/tools/cabana/binaryview.h b/tools/cabana/binaryview.h index e936efc658..d006672793 100644 --- a/tools/cabana/binaryview.h +++ b/tools/cabana/binaryview.h @@ -21,7 +21,7 @@ public: class BinaryViewModel : public QAbstractTableModel { public: BinaryViewModel(QObject *parent) : QAbstractTableModel(parent) {} - void setMessage(const QString &message_id); + void refresh(); void updateState(); QVariant headerData(int section, Qt::Orientation orientation, int role = Qt::DisplayRole) const override; QVariant data(const QModelIndex &index, int role = Qt::DisplayRole) const { return {}; } @@ -36,15 +36,14 @@ public: } struct Item { - QColor bg_color = QApplication::style()->standardPalette().color(QPalette::Base); + QColor bg_color = QColor(102, 86, 169, 0); bool is_msb = false; bool is_lsb = false; - QString val = "0"; + QString val = "-"; QList sigs; }; std::vector items; -private: QString msg_id; const DBCMsg *dbc_msg = nullptr; int row_count = 0; @@ -60,6 +59,7 @@ public: void highlight(const Signal *sig); QSet getOverlappingSignals() const; inline void updateState() { model->updateState(); } + QSize minimumSizeHint() const override; signals: void signalClicked(const Signal *sig); @@ -68,6 +68,7 @@ signals: void resizeSignal(const Signal *sig, int from, int size); private: + void refresh(); std::tuple getSelection(QModelIndex index); void setSelection(const QRect &rect, QItemSelectionModel::SelectionFlags flags) override; void mousePressEvent(QMouseEvent *event) override; diff --git a/tools/cabana/cabana.cc b/tools/cabana/cabana.cc index 4f037ba595..e028e383c2 100644 --- a/tools/cabana/cabana.cc +++ b/tools/cabana/cabana.cc @@ -4,12 +4,16 @@ #include "common/prefix.h" #include "selfdrive/ui/qt/util.h" #include "tools/cabana/mainwin.h" +#include "tools/cabana/route.h" +#include "tools/cabana/streams/livestream.h" +#include "tools/cabana/streams/replaystream.h" int main(int argc, char *argv[]) { QCoreApplication::setApplicationName("Cabana"); QCoreApplication::setAttribute(Qt::AA_ShareOpenGLContexts); initApp(argc, argv); QApplication app(argc, argv); + app.setApplicationDisplayName("Cabana"); QCommandLineParser cmd_parser; cmd_parser.addHelpOption(); @@ -17,29 +21,58 @@ int main(int argc, char *argv[]) { cmd_parser.addOption({"demo", "use a demo route instead of providing your own"}); cmd_parser.addOption({"qcam", "load qcamera"}); cmd_parser.addOption({"ecam", "load wide road camera"}); + cmd_parser.addOption({"stream", "read can messages from live streaming"}); + cmd_parser.addOption({"zmq", "the ip address on which to receive zmq messages", "zmq"}); cmd_parser.addOption({"data_dir", "local directory with routes", "data_dir"}); + cmd_parser.addOption({"no-vipc", "do not output video"}); + cmd_parser.addOption({"dbc", "dbc file to open", "dbc"}); cmd_parser.process(app); - const QStringList args = cmd_parser.positionalArguments(); - if (args.empty() && !cmd_parser.isSet("demo")) { - cmd_parser.showHelp(); - } + std::unique_ptr op_prefix; + std::unique_ptr stream; + + if (cmd_parser.isSet("stream")) { + stream.reset(new LiveStream(&app, cmd_parser.value("zmq"))); + } else { + // TODO: Remove when OpenpilotPrefix supports ZMQ +#ifndef __APPLE__ + op_prefix.reset(new OpenpilotPrefix()); +#endif + uint32_t replay_flags = REPLAY_FLAG_NONE; + if (cmd_parser.isSet("ecam")) { + replay_flags |= REPLAY_FLAG_ECAM; + } else if (cmd_parser.isSet("qcam")) { + replay_flags |= REPLAY_FLAG_QCAMERA; + } else if (cmd_parser.isSet("no-vipc")) { + replay_flags |= REPLAY_FLAG_NO_VIPC; + } + + const QStringList args = cmd_parser.positionalArguments(); + QString route; + if (args.size() > 0) { + route = args.first(); + } else if (cmd_parser.isSet("demo")) { + route = DEMO_ROUTE; + } - const QString route = args.empty() ? DEMO_ROUTE : args.first(); - uint32_t replay_flags = REPLAY_FLAG_NONE; - if (cmd_parser.isSet("ecam")) { - replay_flags |= REPLAY_FLAG_ECAM; - } else if (cmd_parser.isSet("qcam")) { - replay_flags |= REPLAY_FLAG_QCAMERA; + auto replay_stream = new ReplayStream(replay_flags, &app); + stream.reset(replay_stream); + if (route.isEmpty()) { + if (OpenRouteDialog dlg(nullptr); !dlg.exec()) { + return 0; + } + } else if (!replay_stream->loadRoute(route, cmd_parser.value("data_dir"))) { + return 0; + } } - OpenpilotPrefix op_prefix; - CANMessages p(&app); - int ret = 0; - if (p.loadRoute(route, cmd_parser.value("data_dir"), replay_flags)) { - MainWindow w; - w.show(); - ret = app.exec(); + MainWindow w; + + // Load DBC + if (cmd_parser.isSet("dbc")) { + w.loadFile(cmd_parser.value("dbc")); } - return ret; + + w.show(); + return app.exec(); } diff --git a/tools/cabana/canmessages.cc b/tools/cabana/canmessages.cc deleted file mode 100644 index 6e19eb440f..0000000000 --- a/tools/cabana/canmessages.cc +++ /dev/null @@ -1,101 +0,0 @@ -#include "tools/cabana/canmessages.h" -#include "tools/cabana/dbcmanager.h" - -CANMessages *can = nullptr; - -CANMessages::CANMessages(QObject *parent) : QObject(parent) { - can = this; - QObject::connect(this, &CANMessages::received, this, &CANMessages::process, Qt::QueuedConnection); - QObject::connect(&settings, &Settings::changed, this, &CANMessages::settingChanged); -} - -CANMessages::~CANMessages() { - replay->stop(); -} - -static bool event_filter(const Event *e, void *opaque) { - CANMessages *c = (CANMessages *)opaque; - return c->eventFilter(e); -} - -bool CANMessages::loadRoute(const QString &route, const QString &data_dir, uint32_t replay_flags) { - replay = new Replay(route, {"can", "roadEncodeIdx", "wideRoadEncodeIdx", "carParams"}, {}, nullptr, replay_flags, data_dir, this); - replay->setSegmentCacheLimit(settings.cached_segment_limit); - replay->installEventFilter(event_filter, this); - QObject::connect(replay, &Replay::seekedTo, this, &CANMessages::seekedTo); - QObject::connect(replay, &Replay::segmentsMerged, this, &CANMessages::eventsMerged); - QObject::connect(replay, &Replay::streamStarted, this, &CANMessages::streamStarted); - if (replay->load()) { - const auto &segments = replay->route()->segments(); - if (std::none_of(segments.begin(), segments.end(), [](auto &s) { return s.second.rlog.length() > 0; })) { - qWarning() << "no rlogs in route" << route; - return false; - } - replay->start(); - return true; - } - return false; -} - -void CANMessages::process(QHash *messages) { - for (auto it = messages->begin(); it != messages->end(); ++it) { - can_msgs[it.key()] = it.value(); - } - emit updated(); - emit msgsReceived(messages); - delete messages; - processing = false; -} - -bool CANMessages::eventFilter(const Event *event) { - static std::unique_ptr new_msgs = std::make_unique>(); - static double prev_update_ts = 0; - - if (event->which == cereal::Event::Which::CAN) { - double current_sec = replay->currentSeconds(); - if (counters_begin_sec == 0 || counters_begin_sec >= current_sec) { - new_msgs->clear(); - counters.clear(); - counters_begin_sec = current_sec; - } - - auto can_events = event->event.getCan(); - for (const auto &c : can_events) { - QString id = QString("%1:%2").arg(c.getSrc()).arg(c.getAddress(), 1, 16); - CanData &data = (*new_msgs)[id]; - data.ts = current_sec; - data.dat = QByteArray((char *)c.getDat().begin(), c.getDat().size()); - data.count = ++counters[id]; - if (double delta = (current_sec - counters_begin_sec); delta > 0) { - data.freq = data.count / delta; - } - } - - double ts = millis_since_boot(); - if ((ts - prev_update_ts) > (1000.0 / settings.fps) && !processing && !new_msgs->isEmpty()) { - // delay posting CAN message if UI thread is busy - processing = true; - prev_update_ts = ts; - // use pointer to avoid data copy in queued connection. - emit received(new_msgs.release()); - new_msgs.reset(new QHash); - new_msgs->reserve(100); - } - } - return true; -} - -void CANMessages::seekTo(double ts) { - replay->seekTo(std::max(double(0), ts), false); - counters_begin_sec = 0; - emit updated(); -} - -void CANMessages::pause(bool pause) { - replay->pause(pause); - emit (pause ? paused() : resume()); -} - -void CANMessages::settingChanged() { - replay->setSegmentCacheLimit(settings.cached_segment_limit); -} diff --git a/tools/cabana/canmessages.h b/tools/cabana/canmessages.h deleted file mode 100644 index ea43933565..0000000000 --- a/tools/cabana/canmessages.h +++ /dev/null @@ -1,81 +0,0 @@ -#pragma once - -#include - -#include -#include - -#include "opendbc/can/common_dbc.h" -#include "tools/cabana/settings.h" -#include "tools/replay/replay.h" - -struct CanData { - double ts = 0.; - uint32_t count = 0; - uint32_t freq = 0; - QByteArray dat; -}; - -class CANMessages : public QObject { - Q_OBJECT - -public: - CANMessages(QObject *parent); - ~CANMessages(); - bool loadRoute(const QString &route, const QString &data_dir, uint32_t replay_flags = REPLAY_FLAG_NONE); - void seekTo(double ts); - bool eventFilter(const Event *event); - - inline QString routeName() const { return replay->route()->name(); } - inline QString carFingerprint() const { return replay->carFingerprint().c_str(); } - inline VisionStreamType visionStreamType() const { return replay->hasFlag(REPLAY_FLAG_ECAM) ? VISION_STREAM_WIDE_ROAD : VISION_STREAM_ROAD; } - inline double totalSeconds() const { return replay->totalSeconds(); } - inline double routeStartTime() const { return replay->routeStartTime() / (double)1e9; } - inline double currentSec() const { return replay->currentSeconds(); } - inline const CanData &lastMessage(const QString &id) { return can_msgs[id]; } - - inline const Route* route() const { return replay->route(); } - inline const std::vector *events() const { return replay->events(); } - inline void setSpeed(float speed) { replay->setSpeed(speed); } - inline bool isPaused() const { return replay->isPaused(); } - void pause(bool pause); - inline const std::vector> getTimeline() { return replay->getTimeline(); } - -signals: - void paused(); - void resume(); - void seekedTo(double sec); - void streamStarted(); - void eventsMerged(); - void updated(); - void msgsReceived(const QHash *); - void received(QHash *); - -public: - QMap can_msgs; - -protected: - void process(QHash *); - void settingChanged(); - - Replay *replay = nullptr; - std::atomic counters_begin_sec = 0; - std::atomic processing = false; - QHash counters; -}; - -inline QString toHex(const QByteArray &dat) { - return dat.toHex(' ').toUpper(); -} -inline char toHex(uint value) { - return "0123456789ABCDEF"[value & 0xF]; -} - -inline const QString &getColor(int i) { - // TODO: add more colors - static const QString SIGNAL_COLORS[] = {"#9FE2BF", "#40E0D0", "#6495ED", "#CCCCFF", "#FF7F50", "#FFBF00"}; - return SIGNAL_COLORS[i % std::size(SIGNAL_COLORS)]; -} - -// A global pointer referring to the unique CANMessages object -extern CANMessages *can; diff --git a/tools/cabana/chartswidget.cc b/tools/cabana/chartswidget.cc index 952ed2d83f..5063ec9dea 100644 --- a/tools/cabana/chartswidget.cc +++ b/tools/cabana/chartswidget.cc @@ -2,12 +2,15 @@ #include #include -#include +#include +#include #include #include +#include +#include #include +#include #include -#include #include #include @@ -15,26 +18,51 @@ ChartsWidget::ChartsWidget(QWidget *parent) : QWidget(parent) { QVBoxLayout *main_layout = new QVBoxLayout(this); - main_layout->setContentsMargins(0, 0, 0, 0); // toolbar QToolBar *toolbar = new QToolBar(tr("Charts"), this); - title_label = new QLabel(); - title_label->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Preferred); - toolbar->addWidget(title_label); - show_all_values_btn = toolbar->addAction(""); - toolbar->addWidget(range_label = new QLabel()); - reset_zoom_btn = toolbar->addAction("⟲"); - reset_zoom_btn->setToolTip(tr("Reset zoom (drag on chart to zoom X-Axis)")); - remove_all_btn = toolbar->addAction("✖"); + toolbar->setIconSize({16, 16}); + + QAction *new_plot_btn = toolbar->addAction(utils::icon("file-plus"), ""); + new_plot_btn->setToolTip(tr("New Plot")); + toolbar->addWidget(title_label = new QLabel()); + title_label->setContentsMargins(0, 0, 12, 0); + columns_cb = new QComboBox(this); + columns_cb->addItems({"1", "2", "3", "4"}); + columns_lb_action = toolbar->addWidget(new QLabel(tr("Columns:"))); + columns_cb_action = toolbar->addWidget(columns_cb); + + QLabel *stretch_label = new QLabel(this); + stretch_label->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Preferred); + toolbar->addWidget(stretch_label); + + range_lb_action = toolbar->addWidget(range_lb = new QLabel(this)); + range_slider = new QSlider(Qt::Horizontal, this); + range_slider->setToolTip(tr("Set the chart range")); + range_slider->setRange(1, settings.max_cached_minutes * 60); + range_slider->setSingleStep(1); + range_slider->setPageStep(60); // 1 min + range_slider_action = toolbar->addWidget(range_slider); + + reset_zoom_action = toolbar->addWidget(reset_zoom_btn = new QToolButton()); + reset_zoom_btn->setIcon(utils::icon("zoom-out")); + reset_zoom_btn->setToolTip(tr("Reset zoom")); + reset_zoom_btn->setToolButtonStyle(Qt::ToolButtonTextBesideIcon); + + remove_all_btn = toolbar->addAction(utils::icon("x"), ""); remove_all_btn->setToolTip(tr("Remove all charts")); dock_btn = toolbar->addAction(""); main_layout->addWidget(toolbar); // charts + charts_layout = new QGridLayout(); + charts_layout->setSpacing(10); + QWidget *charts_container = new QWidget(this); - charts_layout = new QVBoxLayout(charts_container); - charts_layout->addStretch(); + QVBoxLayout *charts_main_layout = new QVBoxLayout(charts_container); + charts_main_layout->setContentsMargins(0, 0, 0, 0); + charts_main_layout->addLayout(charts_layout); + charts_main_layout->addStretch(0); QScrollArea *charts_scroll = new QScrollArea(this); charts_scroll->setWidgetResizable(true); @@ -42,21 +70,25 @@ ChartsWidget::ChartsWidget(QWidget *parent) : QWidget(parent) { charts_scroll->setHorizontalScrollBarPolicy(Qt::ScrollBarAlwaysOff); main_layout->addWidget(charts_scroll); - max_chart_range = settings.max_chart_x_range; + // init settings use_dark_theme = QApplication::style()->standardPalette().color(QPalette::WindowText).value() > QApplication::style()->standardPalette().color(QPalette::Background).value(); + column_count = std::clamp(settings.chart_column_count, 1, columns_cb->count()); + max_chart_range = std::clamp(settings.chart_range, 1, settings.max_cached_minutes * 60); + display_range = {0, max_chart_range}; + columns_cb->setCurrentIndex(column_count - 1); + range_slider->setValue(max_chart_range); updateToolBar(); - align_charts_timer = new QTimer(this); - align_charts_timer->setSingleShot(true); - align_charts_timer->callOnTimeout(this, &ChartsWidget::alignCharts); - QObject::connect(dbc(), &DBCManager::DBCFileChanged, this, &ChartsWidget::removeAll); - QObject::connect(can, &CANMessages::eventsMerged, this, &ChartsWidget::eventsMerged); - QObject::connect(can, &CANMessages::updated, this, &ChartsWidget::updateState); - QObject::connect(show_all_values_btn, &QAction::triggered, this, &ChartsWidget::showAllData); + QObject::connect(can, &AbstractStream::eventsMerged, this, &ChartsWidget::eventsMerged); + QObject::connect(can, &AbstractStream::updated, this, &ChartsWidget::updateState); + QObject::connect(range_slider, &QSlider::valueChanged, this, &ChartsWidget::setMaxChartRange); + QObject::connect(new_plot_btn, &QAction::triggered, this, &ChartsWidget::newChart); QObject::connect(remove_all_btn, &QAction::triggered, this, &ChartsWidget::removeAll); - QObject::connect(reset_zoom_btn, &QAction::triggered, this, &ChartsWidget::zoomReset); + QObject::connect(reset_zoom_btn, &QToolButton::clicked, this, &ChartsWidget::zoomReset); + QObject::connect(columns_cb, SIGNAL(activated(int)), SLOT(setColumnCount(int))); + QObject::connect(&settings, &Settings::changed, this, &ChartsWidget::settingChanged); QObject::connect(dock_btn, &QAction::triggered, [this]() { emit dock(!docking); docking = !docking; @@ -65,27 +97,15 @@ ChartsWidget::ChartsWidget(QWidget *parent) : QWidget(parent) { } void ChartsWidget::eventsMerged() { - if (auto events = can->events(); events && !events->empty()) { - event_range.first = (events->front()->mono_time / (double)1e9) - can->routeStartTime(); - event_range.second = (events->back()->mono_time / (double)1e9) - can->routeStartTime(); - updateState(); - } -} - -void ChartsWidget::updateDisplayRange() { - auto prev_range = display_range; - double current_sec = can->currentSec(); - if (current_sec < display_range.first || current_sec >= (display_range.second - 5)) { - // reached the end, or seeked to a timestamp out of range. - display_range.first = current_sec - 5; - } - display_range.first = std::floor(std::max(display_range.first, event_range.first) * 10.0) / 10.0; - display_range.second = std::floor(std::min(display_range.first + max_chart_range, event_range.second) * 10.0) / 10.0; - if (prev_range != display_range) { + { + assert(!can->liveStreaming()); QFutureSynchronizer future_synchronizer; - for (auto c : charts) - future_synchronizer.addFuture(QtConcurrent::run(c, &ChartView::setEventsRange, display_range)); + const auto events = can->events(); + for (auto c : charts) { + future_synchronizer.addFuture(QtConcurrent::run(c, &ChartView::updateSeries, nullptr, events, true)); + } } + updateState(); } void ChartsWidget::zoomIn(double min, double max) { @@ -103,71 +123,93 @@ void ChartsWidget::zoomReset() { void ChartsWidget::updateState() { if (charts.isEmpty()) return; + const auto events = can->events(); + if (can->liveStreaming()) { + // appends incoming events to the end of series + for (auto c : charts) { + c->updateSeries(nullptr, events, false); + } + } + + const double cur_sec = can->currentSec(); if (!is_zoomed) { - updateDisplayRange(); - } else if (can->currentSec() < zoomed_range.first || can->currentSec() >= zoomed_range.second) { + double pos = (cur_sec - display_range.first) / std::max(1.0, (display_range.second - display_range.first)); + if (pos < 0 || pos > 0.8) { + display_range.first = std::max(0.0, cur_sec - max_chart_range * 0.1); + } + double max_event_sec = events->empty() ? 0 : (events->back()->mono_time / 1e9 - can->routeStartTime()); + double max_sec = std::min(std::floor(display_range.first + max_chart_range), max_event_sec); + display_range.first = std::max(0.0, max_sec - max_chart_range); + display_range.second = display_range.first + max_chart_range; + } else if (cur_sec < zoomed_range.first || cur_sec >= zoomed_range.second) { + // loop in zoommed range can->seekTo(zoomed_range.first); } + charts_layout->parentWidget()->setUpdatesEnabled(false); const auto &range = is_zoomed ? zoomed_range : display_range; - setUpdatesEnabled(false); for (auto c : charts) { - c->setDisplayRange(range.first, range.second); - c->scene()->invalidate({}, QGraphicsScene::ForegroundLayer); + c->updatePlot(cur_sec, range.first, range.second); } - setUpdatesEnabled(true); + charts_layout->parentWidget()->setUpdatesEnabled(true); } -void ChartsWidget::showAllData() { - bool switch_to_show_all = max_chart_range == settings.max_chart_x_range; - max_chart_range = switch_to_show_all ? settings.cached_segment_limit * 60 - : settings.max_chart_x_range; - max_chart_range = std::min(max_chart_range, (uint32_t)can->totalSeconds()); +void ChartsWidget::setMaxChartRange(int value) { + max_chart_range = settings.chart_range = value; updateToolBar(); updateState(); } void ChartsWidget::updateToolBar() { - int min_range = std::min(settings.max_chart_x_range, (int)can->totalSeconds()); - bool displaying_all = max_chart_range != min_range; - show_all_values_btn->setText(tr("%1 minutes").arg(max_chart_range / 60)); - show_all_values_btn->setToolTip(tr("Click to display %1 data").arg(displaying_all ? tr("%1 minutes").arg(min_range / 60) : tr("ALL cached"))); - show_all_values_btn->setVisible(!is_zoomed); + title_label->setText(tr("Charts: %1").arg(charts.size())); + range_lb->setText(QString("Range: %1:%2 ").arg(max_chart_range / 60, 2, 10, QLatin1Char('0')).arg(max_chart_range % 60, 2, 10, QLatin1Char('0'))); + range_lb_action->setVisible(!is_zoomed); + range_slider_action->setVisible(!is_zoomed); + reset_zoom_action->setVisible(is_zoomed); + reset_zoom_btn->setText(is_zoomed ? tr("Zoomin: %1-%2").arg(zoomed_range.first, 0, 'f', 1).arg(zoomed_range.second, 0, 'f', 1) : ""); remove_all_btn->setEnabled(!charts.isEmpty()); - reset_zoom_btn->setEnabled(is_zoomed); - range_label->setText(is_zoomed ? tr("%1 - %2").arg(zoomed_range.first, 0, 'f', 2).arg(zoomed_range.second, 0, 'f', 2) : ""); - title_label->setText(charts.size() > 0 ? tr("Charts (%1)").arg(charts.size()) : tr("Charts")); - dock_btn->setText(docking ? "⬈" : "⬋"); + dock_btn->setIcon(utils::icon(docking ? "arrow-up-right-square" : "arrow-down-left-square")); dock_btn->setToolTip(docking ? tr("Undock charts") : tr("Dock charts")); } +void ChartsWidget::settingChanged() { + range_slider->setRange(1, settings.max_cached_minutes * 60); + for (auto c : charts) { + c->setFixedHeight(settings.chart_height); + c->setSeriesType(settings.chart_series_type == 0 ? QAbstractSeries::SeriesTypeLine : QAbstractSeries::SeriesTypeScatter); + } +} + ChartView *ChartsWidget::findChart(const QString &id, const Signal *sig) { for (auto c : charts) if (c->hasSeries(id, sig)) return c; return nullptr; } +ChartView *ChartsWidget::createChart() { + auto chart = new ChartView(this); + chart->setFixedHeight(settings.chart_height); + chart->setMinimumWidth(CHART_MIN_WIDTH); + chart->setSizePolicy(QSizePolicy::MinimumExpanding, QSizePolicy::Fixed); + chart->chart()->setTheme(use_dark_theme ? QChart::QChart::ChartThemeDark : QChart::ChartThemeLight); + QObject::connect(chart, &ChartView::remove, [=]() { removeChart(chart); }); + QObject::connect(chart, &ChartView::zoomIn, this, &ChartsWidget::zoomIn); + QObject::connect(chart, &ChartView::zoomReset, this, &ChartsWidget::zoomReset); + QObject::connect(chart, &ChartView::seriesRemoved, this, &ChartsWidget::seriesChanged); + QObject::connect(chart, &ChartView::seriesAdded, this, &ChartsWidget::seriesChanged); + QObject::connect(chart, &ChartView::axisYLabelWidthChanged, this, &ChartsWidget::alignCharts); + charts.push_back(chart); + updateLayout(); + return chart; +} + void ChartsWidget::showChart(const QString &id, const Signal *sig, bool show, bool merge) { setUpdatesEnabled(false); ChartView *chart = findChart(id, sig); if (show && !chart) { - chart = merge && charts.size() > 0 ? charts.back() : nullptr; - if (!chart) { - chart = new ChartView(this); - chart->chart()->setTheme(use_dark_theme ? QChart::QChart::ChartThemeDark : QChart::ChartThemeLight); - chart->setEventsRange(display_range); - auto range = is_zoomed ? zoomed_range : display_range; - chart->setDisplayRange(range.first, range.second); - QObject::connect(chart, &ChartView::remove, [=]() { removeChart(chart); }); - QObject::connect(chart, &ChartView::zoomIn, this, &ChartsWidget::zoomIn); - QObject::connect(chart, &ChartView::zoomReset, this, &ChartsWidget::zoomReset); - QObject::connect(chart, &ChartView::seriesRemoved, this, &ChartsWidget::seriesChanged); - QObject::connect(chart, &ChartView::seriesAdded, this, &ChartsWidget::seriesChanged); - QObject::connect(chart, &ChartView::axisYUpdated, [this]() { align_charts_timer->start(100); }); - charts_layout->insertWidget(0, chart); - charts.push_back(chart); - } + chart = merge && charts.size() > 0 ? charts.back() : createChart(); chart->addSeries(id, sig); + updateState(); } else if (!show && chart) { chart->removeSeries(id, sig); } @@ -175,11 +217,59 @@ void ChartsWidget::showChart(const QString &id, const Signal *sig, bool show, bo setUpdatesEnabled(true); } +void ChartsWidget::setColumnCount(int n) { + n = std::clamp(n + 1, 1, columns_cb->count()); + if (column_count != n) { + column_count = settings.chart_column_count = n; + updateLayout(); + } +} + +void ChartsWidget::updateLayout() { + int n = columns_cb->count(); + for (; n > 1; --n) { + if ((n * CHART_MIN_WIDTH + (n - 1) * charts_layout->spacing()) < charts_layout->geometry().width()) break; + } + + bool show_column_cb = n > 1; + columns_lb_action->setVisible(show_column_cb); + columns_cb_action->setVisible(show_column_cb); + + n = std::min(column_count, n); + if (charts.size() != charts_layout->count() || n != current_column_count) { + current_column_count = n; + charts_layout->parentWidget()->setUpdatesEnabled(false); + for (int i = 0; i < charts.size(); ++i) { + charts_layout->addWidget(charts[charts.size() - i - 1], i / n, i % n); + } + QTimer::singleShot(0, [this]() { charts_layout->parentWidget()->setUpdatesEnabled(true); }); + } +} + +void ChartsWidget::resizeEvent(QResizeEvent *event) { + QWidget::resizeEvent(event); + updateLayout(); +} + +void ChartsWidget::newChart() { + SeriesSelector dlg(tr("New Chart"), this); + if (dlg.exec() == QDialog::Accepted) { + auto items = dlg.seletedItems(); + if (!items.isEmpty()) { + auto c = createChart(); + for (auto it : items) { + c->addSeries(it->msg_id, it->sig); + } + } + } +} + void ChartsWidget::removeChart(ChartView *chart) { charts.removeOne(chart); chart->deleteLater(); updateToolBar(); alignCharts(); + updateLayout(); emit seriesChanged(); } @@ -195,10 +285,11 @@ void ChartsWidget::removeAll() { void ChartsWidget::alignCharts() { int plot_left = 0; for (auto c : charts) { - plot_left = qMax((qreal)plot_left, c->getYAsixLabelWidth()); + plot_left = std::max(plot_left, c->y_label_width); } + plot_left = std::max((plot_left / 10) * 10 + 10, 50); for (auto c : charts) { - c->setPlotAreaLeftPosition(plot_left); + c->updatePlotArea(plot_left); } } @@ -213,17 +304,28 @@ bool ChartsWidget::eventFilter(QObject *obj, QEvent *event) { // ChartView ChartView::ChartView(QWidget *parent) : QChartView(nullptr, parent) { + series_type = settings.chart_series_type == 0 ? QAbstractSeries::SeriesTypeLine : QAbstractSeries::SeriesTypeScatter; + QChart *chart = new QChart(); - chart->setBackgroundRoundness(0); + chart->setBackgroundVisible(false); axis_x = new QValueAxis(this); axis_y = new QValueAxis(this); chart->addAxis(axis_x, Qt::AlignBottom); chart->addAxis(axis_y, Qt::AlignLeft); + chart->legend()->layout()->setContentsMargins(16, 0, 40, 0); chart->legend()->setShowToolTips(true); - chart->layout()->setContentsMargins(0, 0, 0, 0); + chart->setMargins({0, 0, 0, 0}); + + background = new QGraphicsRectItem(chart); + background->setBrush(Qt::white); + background->setPen(Qt::NoPen); + background->setZValue(chart->zValue() - 1); + + move_icon = new QGraphicsPixmapItem(utils::icon("grip-horizontal"), chart); + move_icon->setToolTip(tr("Drag and drop to combine charts")); QToolButton *remove_btn = new QToolButton(); - remove_btn->setText("X"); + remove_btn->setIcon(utils::icon("x")); remove_btn->setAutoRaise(true); remove_btn->setToolTip(tr("Remove Chart")); close_btn_proxy = new QGraphicsProxyWidget(chart); @@ -231,43 +333,38 @@ ChartView::ChartView(QWidget *parent) : QChartView(nullptr, parent) { close_btn_proxy->setZValue(chart->zValue() + 11); QToolButton *manage_btn = new QToolButton(); - manage_btn->setText("🔧"); + manage_btn->setToolButtonStyle(Qt::ToolButtonIconOnly); + manage_btn->setIcon(utils::icon("list")); manage_btn->setAutoRaise(true); - manage_btn->setToolTip(tr("Manage series")); + QMenu *menu = new QMenu(this); + line_series_action = menu->addAction(tr("Line"), [this]() { setSeriesType(QAbstractSeries::SeriesTypeLine); }); + line_series_action->setCheckable(true); + line_series_action->setChecked(series_type == QAbstractSeries::SeriesTypeLine); + scatter_series_action = menu->addAction(tr("Scatter"), [this]() { setSeriesType(QAbstractSeries::SeriesTypeScatter); }); + scatter_series_action->setCheckable(true); + scatter_series_action->setChecked(series_type == QAbstractSeries::SeriesTypeScatter); + menu->addSeparator(); + menu->addAction(tr("Manage series"), this, &ChartView::manageSeries); + manage_btn->setMenu(menu); + manage_btn->setPopupMode(QToolButton::InstantPopup); manage_btn_proxy = new QGraphicsProxyWidget(chart); manage_btn_proxy->setWidget(manage_btn); manage_btn_proxy->setZValue(chart->zValue() + 11); setChart(chart); setRenderHint(QPainter::Antialiasing); - setRubberBand(QChartView::HorizontalRubberBand); - updateFromSettings(); + // TODO: enable zoomIn/seekTo in live streaming mode. + setRubberBand(can->liveStreaming() ? QChartView::NoRubberBand : QChartView::HorizontalRubberBand); QObject::connect(dbc(), &DBCManager::signalRemoved, this, &ChartView::signalRemoved); QObject::connect(dbc(), &DBCManager::signalUpdated, this, &ChartView::signalUpdated); QObject::connect(dbc(), &DBCManager::msgRemoved, this, &ChartView::msgRemoved); QObject::connect(dbc(), &DBCManager::msgUpdated, this, &ChartView::msgUpdated); - QObject::connect(&settings, &Settings::changed, this, &ChartView::updateFromSettings); QObject::connect(remove_btn, &QToolButton::clicked, this, &ChartView::remove); - QObject::connect(manage_btn, &QToolButton::clicked, this, &ChartView::manageSeries); -} - -qreal ChartView::getYAsixLabelWidth() const { - QFontMetrics fm(axis_y->labelsFont()); - int n = qMax(int(-qFloor(std::log10((axis_y->max() - axis_y->min()) / (axis_y->tickCount() - 1)))), 0) + 1; - return qMax(fm.width(QString::number(axis_y->min(), 'f', n)), fm.width(QString::number(axis_y->max(), 'f', n))) + 20; -} - -void ChartView::setPlotAreaLeftPosition(int pos) { - if (std::ceil(chart()->plotArea().left()) != pos) { - const float left_margin = chart()->margins().left() + pos - chart()->plotArea().left(); - chart()->setMargins(QMargins(left_margin, 11, 11, 11)); - } } void ChartView::addSeries(const QString &msg_id, const Signal *sig) { - QLineSeries *series = new QLineSeries(this); - series->setUseOpenGL(true); + QXYSeries *series = createSeries(series_type, getColor(sig)); chart()->addSeries(series); series->attachAxis(axis_x); series->attachAxis(axis_y); @@ -275,14 +372,14 @@ void ChartView::addSeries(const QString &msg_id, const Signal *sig) { sigs.push_back({.msg_id = msg_id, .address = address, .source = source, .sig = sig, .series = series}); updateTitle(); updateSeries(sig); - updateAxisY(); + updateSeriesPoints(); emit seriesAdded(msg_id, sig); } void ChartView::removeSeries(const QString &msg_id, const Signal *sig) { auto it = std::find_if(sigs.begin(), sigs.end(), [&](auto &s) { return s.msg_id == msg_id && s.sig == sig; }); if (it != sigs.end()) { - it = removeSeries(it); + it = removeItem(it); } } @@ -290,7 +387,7 @@ bool ChartView::hasSeries(const QString &msg_id, const Signal *sig) const { return std::any_of(sigs.begin(), sigs.end(), [&](auto &s) { return s.msg_id == msg_id && s.sig == sig; }); } -QList::iterator ChartView::removeSeries(const QList::iterator &it) { +QList::iterator ChartView::removeItem(const QList::iterator &it) { chart()->removeSeries(it->series); it->series->deleteLater(); QString msg_id = it->msg_id; @@ -310,13 +407,12 @@ void ChartView::signalUpdated(const Signal *sig) { updateTitle(); // TODO: don't update series if only name changed. updateSeries(sig); - updateAxisY(); } } void ChartView::signalRemoved(const Signal *sig) { for (auto it = sigs.begin(); it != sigs.end(); /**/) { - it = (it->sig == sig) ? removeSeries(it) : ++it; + it = (it->sig == sig) ? removeItem(it) : ++it; } } @@ -327,35 +423,30 @@ void ChartView::msgUpdated(uint32_t address) { void ChartView::msgRemoved(uint32_t address) { for (auto it = sigs.begin(); it != sigs.end(); /**/) { - it = (it->address == address) ? removeSeries(it) : ++it; + it = (it->address == address) ? removeItem(it) : ++it; } } void ChartView::manageSeries() { - SeriesSelector dlg(this); + SeriesSelector dlg(tr("Mange Chart"), this); for (auto &s : sigs) { - dlg.addSeries(s.msg_id, msgName(s.msg_id), QString::fromStdString(s.sig->name)); + dlg.addSelected(s.msg_id, s.sig); } - - int ret = dlg.exec(); - if (ret == QDialog::Accepted) { - QList series_list = dlg.series(); - if (series_list.isEmpty()) { + if (dlg.exec() == QDialog::Accepted) { + auto items = dlg.seletedItems(); + if (items.isEmpty()) { emit remove(); } else { - for (auto &s : series_list) { - if (auto m = dbc()->msg(s[0])) { - auto it = m->sigs.find(s[2]); - if (it != m->sigs.end() && !hasSeries(s[0], &(it->second))) { - addSeries(s[0], &(it->second)); - } + for (auto s : items) { + if (!hasSeries(s->msg_id, s->sig)) { + addSeries(s->msg_id, s->sig); } } for (auto it = sigs.begin(); it != sigs.end(); /**/) { - bool exists = std::any_of(series_list.cbegin(), series_list.cend(), [&](auto &s) { - return s[0] == it->msg_id && s[2] == it->sig->name.c_str(); + bool exists = std::any_of(items.cbegin(), items.cend(), [&](auto &s) { + return s->msg_id == it->msg_id && s->sig == it->sig; }); - it = exists ? ++it : removeSeries(it); + it = exists ? ++it : removeItem(it); } } } @@ -363,112 +454,153 @@ void ChartView::manageSeries() { void ChartView::resizeEvent(QResizeEvent *event) { QChartView::resizeEvent(event); + updatePlotArea(align_to); int x = event->size().width() - close_btn_proxy->size().width() - 11; close_btn_proxy->setPos(x, 8); manage_btn_proxy->setPos(x - manage_btn_proxy->size().width() - 5, 8); + move_icon->setPos(11, 8); } -void ChartView::updateTitle() { - for (auto &s : sigs) { - s.series->setName(QString("%1 %2 %3").arg(s.sig->name.c_str()).arg(msgName(s.msg_id)).arg(s.msg_id)); +void ChartView::updatePlotArea(int left) { + QRect r = rect(); + if (align_to != left || r != background->rect()) { + align_to = left; + background->setRect(r); + chart()->legend()->setGeometry(QRect(r.left(), r.top(), r.width(), 45)); + chart()->setPlotArea(QRect(align_to, r.top() + 45, r.width() - align_to - 22, r.height() - 80)); + chart()->layout()->invalidate(); } } -void ChartView::updateFromSettings() { - setFixedHeight(settings.chart_height); -} - -void ChartView::setEventsRange(const std::pair &range) { - if (range != events_range) { - events_range = range; - updateSeries(); +void ChartView::updateTitle() { + for (QLegendMarker *marker : chart()->legend()->markers()) { + QObject::connect(marker, &QLegendMarker::clicked, this, &ChartView::handleMarkerClicked, Qt::UniqueConnection); + } + for (auto &s : sigs) { + auto decoration = s.series->isVisible() ? "none" : "line-through"; + s.series->setName(QString("%2 %3 %4").arg(decoration, s.sig->name.c_str(), msgName(s.msg_id), s.msg_id)); } } -void ChartView::setDisplayRange(double min, double max) { +void ChartView::updatePlot(double cur, double min, double max) { + cur_sec = cur; if (min != axis_x->min() || max != axis_x->max()) { axis_x->setRange(min, max); updateAxisY(); + updateSeriesPoints(); } + + scene()->invalidate({}, QGraphicsScene::ForegroundLayer); } -void ChartView::updateSeries(const Signal *sig) { - auto events = can->events(); - if (!events || sigs.isEmpty()) return; +void ChartView::updateSeriesPoints() { + // Show points when zoomed in enough + for (auto &s : sigs) { + auto begin = std::lower_bound(s.vals.begin(), s.vals.end(), axis_x->min(), [](auto &p, double x) { return p.x() < x; }); + auto end = std::lower_bound(begin, s.vals.end(), axis_x->max(), [](auto &p, double x) { return p.x() < x; }); + + int num_points = std::max(end - begin, 1); + int pixels_per_point = width() / num_points; + if (series_type == QAbstractSeries::SeriesTypeScatter) { + ((QScatterSeries *)s.series)->setMarkerSize(std::clamp(pixels_per_point / 3, 1, 8)); + } else { + s.series->setPointsVisible(pixels_per_point > 20); + } + } +} + +void ChartView::updateSeries(const Signal *sig, const std::vector *events, bool clear) { + events = events ? events : can->events(); for (auto &s : sigs) { if (!sig || s.sig == sig) { - s.vals.clear(); - s.vals.reserve((events_range.second - events_range.first) * 1000); // [n]seconds * 1000hz - s.min_y = std::numeric_limits::max(); - s.max_y = std::numeric_limits::lowest(); - - double route_start_time = can->routeStartTime(); - Event begin_event(cereal::Event::Which::INIT_DATA, (route_start_time + events_range.first) * 1e9); - auto begin = std::lower_bound(events->begin(), events->end(), &begin_event, Event::lessThan()); - double end_ns = (route_start_time + events_range.second) * 1e9; - - for (auto it = begin; it != events->end() && (*it)->mono_time <= end_ns; ++it) { - if ((*it)->which == cereal::Event::Which::CAN) { - for (const auto &c : (*it)->event.getCan()) { - if (s.source == c.getSrc() && s.address == c.getAddress()) { - auto dat = c.getDat(); - double value = get_raw_value((uint8_t *)dat.begin(), dat.size(), *s.sig); - double ts = ((*it)->mono_time / (double)1e9) - route_start_time; // seconds - s.vals.push_back({ts, value}); - - if (value < s.min_y) s.min_y = value; - if (value > s.max_y) s.max_y = value; + if (clear) { + s.vals.clear(); + s.vals.reserve(settings.max_cached_minutes * 60 * 100); // [n]seconds * 100hz + s.last_value_mono_time = 0; + } + s.series->setColor(getColor(s.sig)); + + struct Chunk { + std::vector::const_iterator first, second; + QVector vals; + }; + // split into one minitue chunks + QVector chunks; + Event begin_event(cereal::Event::Which::INIT_DATA, s.last_value_mono_time); + auto begin = std::upper_bound(events->begin(), events->end(), &begin_event, Event::lessThan()); + for (auto it = begin, second = begin; it != events->end(); it = second) { + second = std::lower_bound(it, events->end(), (*it)->mono_time + 1e9 * 60, [](auto &e, uint64_t ts) { return e->mono_time < ts; }); + chunks.push_back({it, second}); + } + + QtConcurrent::blockingMap(chunks, [&](Chunk &chunk) { + chunk.vals.reserve(60 * 100); // 100 hz + double route_start_time = can->routeStartTime(); + for (auto it = chunk.first; it != chunk.second; ++it) { + if ((*it)->which == cereal::Event::Which::CAN) { + for (const auto &c : (*it)->event.getCan()) { + if (s.address == c.getAddress() && s.source == c.getSrc()) { + auto dat = c.getDat(); + double value = get_raw_value((uint8_t *)dat.begin(), dat.size(), *s.sig); + double ts = ((*it)->mono_time / (double)1e9) - route_start_time; // seconds + chunk.vals.push_back({ts, value}); + } } } } + }); + for (auto &c : chunks) { + s.vals.append(c.vals); + } + if (events->size()) { + s.last_value_mono_time = events->back()->mono_time; } s.series->replace(s.vals); } } + updateAxisY(); } // auto zoom on yaxis void ChartView::updateAxisY() { if (sigs.isEmpty()) return; - double min_y = std::numeric_limits::max(); - double max_y = std::numeric_limits::lowest(); - if (events_range == std::pair{axis_x->min(), axis_x->max()}) { - for (auto &s : sigs) { - if (s.min_y < min_y) min_y = s.min_y; - if (s.max_y > max_y) max_y = s.max_y; - } - } else { - for (auto &s : sigs) { - auto begin = std::lower_bound(s.vals.begin(), s.vals.end(), axis_x->min(), [](auto &p, double x) { return p.x() < x; }); - for (auto it = begin; it != s.vals.end() && it->x() <= axis_x->max(); ++it) { - if (it->y() < min_y) min_y = it->y(); - if (it->y() > max_y) max_y = it->y(); - } + double min = std::numeric_limits::max(); + double max = std::numeric_limits::lowest(); + for (auto &s : sigs) { + if (!s.series->isVisible()) continue; + + auto first = std::lower_bound(s.vals.begin(), s.vals.end(), axis_x->min(), [](auto &p, double x) { return p.x() < x; }); + auto last = std::lower_bound(first, s.vals.end(), axis_x->max(), [](auto &p, double x) { return p.x() < x; }); + for (auto it = first; it != last; ++it) { + if (it->y() < min) min = it->y(); + if (it->y() > max) max = it->y(); } } + if (min == std::numeric_limits::max()) min = 0; + if (max == std::numeric_limits::lowest()) max = 0; - if (min_y == std::numeric_limits::max()) min_y = 0; - if (max_y == std::numeric_limits::lowest()) max_y = 0; - if (max_y == min_y) { - axis_y->setRange(min_y - 1, max_y + 1); - } else { - double range = max_y - min_y; - applyNiceNumbers(min_y - range * 0.05, max_y + range * 0.05); + double delta = std::abs(max - min) < 1e-3 ? 1 : (max - min) * 0.05; + auto [min_y, max_y, tick_count] = getNiceAxisNumbers(min - delta, max + delta, axis_y->tickCount()); + if (min_y != axis_y->min() || max_y != axis_y->max() || y_label_width == 0) { + axis_y->setRange(min_y, max_y); + axis_y->setTickCount(tick_count); + + QFontMetrics fm(axis_y->labelsFont()); + int n = qMax(int(-qFloor(std::log10((max_y - min_y) / (tick_count - 1)))), 0) + 1; + y_label_width = qMax(fm.width(QString::number(min_y, 'f', n)), fm.width(QString::number(max_y, 'f', n))) + 20; // left margin 20 + emit axisYLabelWidthChanged(y_label_width); } - emit axisYUpdated(); } -void ChartView::applyNiceNumbers(qreal min, qreal max) { - int tick_count = axis_y->tickCount(); +std::tuple ChartView::getNiceAxisNumbers(qreal min, qreal max, int tick_count) { qreal range = niceNumber((max - min), true); // range with ceiling qreal step = niceNumber(range / (tick_count - 1), false); min = qFloor(min / step); max = qCeil(max / step); tick_count = int(max - min) + 1; - axis_y->setRange(min * step, max * step); - axis_y->setTickCount(tick_count); + return {min * step, max * step, tick_count}; } // nice numbers can be expressed as form of 1*10^n, 2* 10^n or 5*10^n @@ -490,27 +622,51 @@ qreal ChartView::niceNumber(qreal x, bool ceiling) { } void ChartView::leaveEvent(QEvent *event) { - track_pt = {0, 0}; + track_pts.clear(); scene()->update(); QChartView::leaveEvent(event); } +void ChartView::mousePressEvent(QMouseEvent *event) { + if (event->button() == Qt::LeftButton && move_icon->sceneBoundingRect().contains(event->pos())) { + QMimeData *mimeData = new QMimeData; + mimeData->setData(mime_type, QByteArray::number((qulonglong)this)); + QDrag *drag = new QDrag(this); + drag->setMimeData(mimeData); + drag->setPixmap(grab()); + drag->setHotSpot(event->pos()); + Qt::DropAction dropAction = drag->exec(Qt::CopyAction | Qt::MoveAction, Qt::MoveAction); + if (dropAction == Qt::MoveAction) { + return; + } + } else { + QChartView::mousePressEvent(event); + } +} + void ChartView::mouseReleaseEvent(QMouseEvent *event) { auto rubber = findChild(); if (event->button() == Qt::LeftButton && rubber && rubber->isVisible()) { rubber->hide(); QRectF rect = rubber->geometry().normalized(); - double min = std::floor(chart()->mapToValue(rect.topLeft()).x() * 10.0) / 10.0; - double max = std::floor(chart()->mapToValue(rect.bottomRight()).x() * 10.0) / 10.0; + double min = chart()->mapToValue(rect.topLeft()).x(); + double max = chart()->mapToValue(rect.bottomRight()).x(); + + // Prevent zooming/seeking past the end of the route + min = std::clamp(min, 0., can->totalSeconds()); + max = std::clamp(max, 0., can->totalSeconds()); + + double min_rounded = std::floor(min * 10.0) / 10.0; + double max_rounded = std::floor(max * 10.0) / 10.0; if (rubber->width() <= 0) { // no rubber dragged, seek to mouse position can->seekTo(min); - } else if ((max - min) >= 0.5) { + } else if ((max_rounded - min_rounded) >= 0.5) { // zoom in if selected range is greater than 0.5s - emit zoomIn(min, max); + emit zoomIn(min_rounded, max_rounded); } event->accept(); - } else if (event->button() == Qt::RightButton) { + } else if (!can->liveStreaming() && event->button() == Qt::RightButton) { emit zoomReset(); event->accept(); } else { @@ -522,84 +678,191 @@ void ChartView::mouseMoveEvent(QMouseEvent *ev) { auto rubber = findChild(); bool is_zooming = rubber && rubber->isVisible(); const auto plot_area = chart()->plotArea(); - track_pt = {0, 0}; + track_pts.clear(); if (!is_zooming && plot_area.contains(ev->pos())) { + track_pts.resize(sigs.size()); QStringList text_list; const double sec = chart()->mapToValue(ev->pos()).x(); - for (auto &s : sigs) { + for (int i = 0; i < sigs.size(); ++i) { QString value = "--"; // use reverse iterator to find last item <= sec. - auto it = std::lower_bound(s.vals.rbegin(), s.vals.rend(), sec, [](auto &p, double x) { return p.x() > x; }); - if (it != s.vals.rend() && it->x() >= axis_x->min()) { + auto it = std::lower_bound(sigs[i].vals.rbegin(), sigs[i].vals.rend(), sec, [](auto &p, double x) { return p.x() > x; }); + if (it != sigs[i].vals.rend() && it->x() >= axis_x->min()) { value = QString::number(it->y()); - auto value_pos = chart()->mapToPosition(*it); - if (value_pos.x() > track_pt.x()) track_pt = value_pos; + track_pts[i] = chart()->mapToPosition(*it); } - text_list.push_back(QString(" %1 : %2 ").arg(sigs.size() > 1 ? s.sig->name.c_str() : "Value").arg(value)); + text_list.push_back(QString("■ %2: %3").arg(sigs[i].series->color().name(), sigs[i].sig->name.c_str(), value)); } - if (track_pt.x() == 0) track_pt = ev->pos(); - QString text = QString("
 Time: %1  
%2
") - .arg(chart()->mapToValue(track_pt).x(), 0, 'f', 3) - .arg(text_list.join("
")); - QPoint pt((int)track_pt.x() + 20, plot_area.top() - 20); - QToolTip::showText(mapToGlobal(pt), text, this, plot_area.toRect()); + auto max = std::max_element(track_pts.begin(), track_pts.end(), [](auto &a, auto &b) { return a.x() < b.x(); }); + auto pt = (max == track_pts.end()) ? ev->pos() : *max; + text_list.push_front(QString::number(chart()->mapToValue(pt).x(), 'f', 3)); + QPointF tooltip_pt(pt.x() + 12, plot_area.top() - 20); + QToolTip::showText(mapToGlobal(tooltip_pt.toPoint()), pt.isNull() ? "" : text_list.join("
"), this, plot_area.toRect()); scene()->update(); } else { QToolTip::hideText(); } + QChartView::mouseMoveEvent(ev); + if (is_zooming) { + QRect rubber_rect = rubber->geometry(); + rubber_rect.setLeft(std::max(rubber_rect.left(), (int)plot_area.left())); + rubber_rect.setRight(std::min(rubber_rect.right(), (int)plot_area.right())); + if (rubber_rect != rubber->geometry()) { + rubber->setGeometry(rubber_rect); + } + } +} + +void ChartView::dragMoveEvent(QDragMoveEvent *event) { + if (event->mimeData()->hasFormat(mime_type)) { + event->setDropAction(event->source() == this ? Qt::MoveAction : Qt::CopyAction); + event->accept(); + } else { + event->ignore(); + } +} + +void ChartView::dropEvent(QDropEvent *event) { + if (event->mimeData()->hasFormat(mime_type)) { + if (event->source() == this) { + event->setDropAction(Qt::MoveAction); + event->accept(); + } else { + ChartView *source_chart = (ChartView *)event->source(); + for (auto &s : source_chart->sigs) { + addSeries(s.msg_id, s.sig); + } + emit source_chart->remove(); + event->acceptProposedAction(); + } + } else { + event->ignore(); + } } void ChartView::drawForeground(QPainter *painter, const QRectF &rect) { - qreal x = chart()->mapToPosition(QPointF{can->currentSec(), 0}).x(); + qreal x = chart()->mapToPosition(QPointF{cur_sec, 0}).x(); + x = std::clamp(x, chart()->plotArea().left(), chart()->plotArea().right()); qreal y1 = chart()->plotArea().top() - 2; qreal y2 = chart()->plotArea().bottom() + 2; painter->setPen(QPen(chart()->titleBrush().color(), 2)); painter->drawLine(QPointF{x, y1}, QPointF{x, y2}); - if (!track_pt.isNull()) { + + auto max = std::max_element(track_pts.begin(), track_pts.end(), [](auto &a, auto &b) { return a.x() < b.x(); }); + if (max != track_pts.end() && !max->isNull()) { painter->setPen(QPen(Qt::darkGray, 1, Qt::DashLine)); - painter->drawLine(QPointF{track_pt.x(), y1}, QPointF{track_pt.x(), y2}); - painter->setBrush(Qt::darkGray); - painter->drawEllipse(track_pt, 5, 5); + painter->drawLine(QPointF{max->x(), y1}, QPointF{max->x(), y2}); + painter->setPen(Qt::NoPen); + for (int i = 0; i < track_pts.size(); ++i) { + if (!track_pts[i].isNull() && i < sigs.size()) { + painter->setBrush(sigs[i].series->color().darker(125)); + painter->drawEllipse(track_pts[i], 5.5, 5.5); + } + } + } + + // paint points. OpenGL mode lacks certain features (such as showing points) + painter->setPen(Qt::NoPen); + for (auto &s : sigs) { + if (s.series->useOpenGL() && s.series->isVisible() && s.series->pointsVisible()) { + auto first = std::lower_bound(s.vals.begin(), s.vals.end(), axis_x->min(), [](auto &p, double x) { return p.x() < x; }); + auto last = std::lower_bound(first, s.vals.end(), axis_x->max(), [](auto &p, double x) { return p.x() < x; }); + for (auto it = first; it != last; ++it) { + painter->setBrush(s.series->color()); + painter->drawEllipse(chart()->mapToPosition(*it), 4, 4); + } + } } } -// SeriesSelector +QXYSeries *ChartView::createSeries(QAbstractSeries::SeriesType type, QColor color) { + QXYSeries *series = nullptr; + if (type == QAbstractSeries::SeriesTypeLine) { + series = new QLineSeries(this); + } else { + series = new QScatterSeries(this); + } + series->setColor(color); + // TODO: Due to a bug in CameraWidget the camera frames + // are drawn instead of the graphs on MacOS. Re-enable OpenGL when fixed +#ifndef __APPLE__ + series->setUseOpenGL(true); + // Qt doesn't properly apply device pixel ratio in OpenGL mode + QPen pen = series->pen(); + pen.setWidth(2.0 * qApp->devicePixelRatio()); + series->setPen(pen); +#endif + return series; +} + +void ChartView::setSeriesType(QAbstractSeries::SeriesType type) { + line_series_action->setChecked(type == QAbstractSeries::SeriesTypeLine); + scatter_series_action->setChecked(type == QAbstractSeries::SeriesTypeScatter); + if (type != series_type) { + series_type = type; + for (auto &s : sigs) { + chart()->removeSeries(s.series); + s.series->deleteLater(); + } + for (auto &s : sigs) { + auto series = createSeries(series_type, getColor(s.sig)); + chart()->addSeries(series); + series->attachAxis(axis_x); + series->attachAxis(axis_y); + series->replace(s.vals); + s.series = series; + } + updateSeriesPoints(); + updateTitle(); + } +} + +void ChartView::handleMarkerClicked() { + auto marker = qobject_cast(sender()); + Q_ASSERT(marker); + if (sigs.size() > 1) { + auto series = marker->series(); + series->setVisible(!series->isVisible()); + marker->setVisible(true); + updateAxisY(); + updateTitle(); + } +} -SeriesSelector::SeriesSelector(QWidget *parent) { - setWindowTitle(tr("Manage Chart Series")); - QHBoxLayout *contents_layout = new QHBoxLayout(); +// SeriesSelector - QVBoxLayout *left_layout = new QVBoxLayout(); - left_layout->addWidget(new QLabel(tr("Select Signals:"))); +SeriesSelector::SeriesSelector(QString title, QWidget *parent) : QDialog(parent) { + setWindowTitle(title); + QGridLayout *main_layout = new QGridLayout(this); - msgs_combo = new QComboBox(this); + // left column + main_layout->addWidget(new QLabel(tr("Available Signals")), 0, 0); + main_layout->addWidget(msgs_combo = new QComboBox(this), 1, 0); msgs_combo->setEditable(true); - msgs_combo->lineEdit()->setPlaceholderText(tr("Select Msg")); + msgs_combo->lineEdit()->setPlaceholderText(tr("Select a msg...")); msgs_combo->setInsertPolicy(QComboBox::NoInsert); msgs_combo->completer()->setCompletionMode(QCompleter::PopupCompletion); msgs_combo->completer()->setFilterMode(Qt::MatchContains); - left_layout->addWidget(msgs_combo); - sig_list = new QListWidget(this); - sig_list->setSortingEnabled(true); - sig_list->setToolTip(tr("Double click on an item to add signal to chart")); - left_layout->addWidget(sig_list); - - QVBoxLayout *right_layout = new QVBoxLayout(); - right_layout->addWidget(new QLabel(tr("Chart Signals:"))); - chart_series = new QListWidget(this); - chart_series->setSortingEnabled(true); - chart_series->setToolTip(tr("Double click on an item to remove signal from chart")); - right_layout->addWidget(chart_series); - contents_layout->addLayout(left_layout); - contents_layout->addLayout(right_layout); + main_layout->addWidget(available_list = new QListWidget(this), 2, 0); - auto buttonBox = new QDialogButtonBox(QDialogButtonBox::Ok | QDialogButtonBox::Cancel); + // buttons + QVBoxLayout *btn_layout = new QVBoxLayout(); + QPushButton *add_btn = new QPushButton(utils::icon("chevron-right"), "", this); + QPushButton *remove_btn = new QPushButton(utils::icon("chevron-left"), "", this); + btn_layout->addStretch(0); + btn_layout->addWidget(add_btn); + btn_layout->addWidget(remove_btn); + btn_layout->addStretch(0); + main_layout->addLayout(btn_layout, 0, 1, 3, 1); - QVBoxLayout *main_layout = new QVBoxLayout(this); - main_layout->addLayout(contents_layout); - main_layout->addWidget(buttonBox); + // right column + main_layout->addWidget(new QLabel(tr("Selected Signals")), 0, 2); + main_layout->addWidget(selected_list = new QListWidget(this), 1, 2, 2, 1); + + auto buttonBox = new QDialogButtonBox(QDialogButtonBox::Ok | QDialogButtonBox::Cancel); + main_layout->addWidget(buttonBox, 3, 2); for (auto it = can->can_msgs.cbegin(); it != can->can_msgs.cend(); ++it) { if (auto m = dbc()->msg(it.key())) { @@ -607,56 +870,59 @@ SeriesSelector::SeriesSelector(QWidget *parent) { } } msgs_combo->model()->sort(0); - + msgs_combo->setCurrentIndex(-1); + + QObject::connect(msgs_combo, qOverload(&QComboBox::currentIndexChanged), this, &SeriesSelector::updateAvailableList); + QObject::connect(available_list, &QListWidget::currentRowChanged, [=](int row) { add_btn->setEnabled(row != -1); }); + QObject::connect(selected_list, &QListWidget::currentRowChanged, [=](int row) { remove_btn->setEnabled(row != -1); }); + QObject::connect(available_list, &QListWidget::itemDoubleClicked, this, &SeriesSelector::add); + QObject::connect(selected_list, &QListWidget::itemDoubleClicked, this, &SeriesSelector::remove); + QObject::connect(add_btn, &QPushButton::clicked, [this]() { if (auto item = available_list->currentItem()) add(item); }); + QObject::connect(remove_btn, &QPushButton::clicked, [this]() { if (auto item = selected_list->currentItem()) remove(item);}); QObject::connect(buttonBox, &QDialogButtonBox::accepted, this, &QDialog::accept); QObject::connect(buttonBox, &QDialogButtonBox::rejected, this, &QDialog::reject); - QObject::connect(msgs_combo, SIGNAL(currentIndexChanged(int)), SLOT(msgSelected(int))); - QObject::connect(sig_list, &QListWidget::itemDoubleClicked, this, &SeriesSelector::addSignal); - QObject::connect(chart_series, &QListWidget::itemDoubleClicked, [](QListWidgetItem *item) { delete item; }); +} - if (int index = msgs_combo->currentIndex(); index >= 0) { - msgSelected(index); +void SeriesSelector::add(QListWidgetItem *item) { + auto it = (ListItem *)item; + addItemToList(selected_list, it->msg_id, it->sig, true); + delete item; +} + +void SeriesSelector::remove(QListWidgetItem *item) { + auto it = (ListItem *)item; + if (it->msg_id == msgs_combo->currentData().toString()) { + addItemToList(available_list, it->msg_id, it->sig); } + delete item; } -void SeriesSelector::msgSelected(int index) { +void SeriesSelector::updateAvailableList(int index) { + if (index == -1) return; + available_list->clear(); QString msg_id = msgs_combo->itemData(index).toString(); - sig_list->clear(); - if (auto m = dbc()->msg(msg_id)) { - for (auto &[name, s] : m->sigs) { - QStringList data({msg_id, m->name, name}); - QListWidgetItem *item = new QListWidgetItem(name, sig_list); - item->setData(Qt::UserRole, data); - sig_list->addItem(item); + auto selected_items = seletedItems(); + for (auto &[name, s] : dbc()->msg(msg_id)->sigs) { + bool is_selected = std::any_of(selected_items.begin(), selected_items.end(), [=, sig=&s](auto it) { return it->msg_id == msg_id && it->sig == sig; }); + if (!is_selected) { + addItemToList(available_list, msg_id, &s); } } } -void SeriesSelector::addSignal(QListWidgetItem *item) { - QStringList data = item->data(Qt::UserRole).toStringList(); - addSeries(data[0], data[1], data[2]); -} +void SeriesSelector::addItemToList(QListWidget *parent, const QString id, const Signal *sig, bool show_msg_name) { + QString text = QString("■ %1").arg(getColor(sig).name(), sig->name.c_str()); + if (show_msg_name) text += QString(" %0 %1").arg(msgName(id), id); -void SeriesSelector::addSeries(const QString &id, const QString &msg_name, const QString &sig_name) { - QStringList data({id, msg_name, sig_name}); - for (int i = 0; i < chart_series->count(); ++i) { - if (chart_series->item(i)->data(Qt::UserRole).toStringList() == data) { - return; - } - } - QListWidgetItem *new_item = new QListWidgetItem(chart_series); - new_item->setData(Qt::UserRole, data); - chart_series->addItem(new_item); - QLabel *label = new QLabel(QString("%0 %1 %2").arg(data[2]).arg(data[1]).arg(data[0]), chart_series); + QLabel *label = new QLabel(text); label->setContentsMargins(5, 0, 5, 0); + auto new_item = new ListItem(id, sig, parent); new_item->setSizeHint(label->sizeHint()); - chart_series->setItemWidget(new_item, label); + parent->setItemWidget(new_item, label); } -QList SeriesSelector::series() { - QList ret; - for (int i = 0; i < chart_series->count(); ++i) { - ret.push_back(chart_series->item(i)->data(Qt::UserRole).toStringList()); - } +QList SeriesSelector::seletedItems() { + QList ret; + for (int i = 0; i < selected_list->count(); ++i) ret.push_back((ListItem *)selected_list->item(i)); return ret; } diff --git a/tools/cabana/chartswidget.h b/tools/cabana/chartswidget.h index 819432920b..25949dd654 100644 --- a/tools/cabana/chartswidget.h +++ b/tools/cabana/chartswidget.h @@ -1,21 +1,27 @@ #pragma once #include -#include +#include +#include #include #include +#include #include -#include -#include +#include +#include #include +#include #include +#include #include -#include "tools/cabana/canmessages.h" #include "tools/cabana/dbcmanager.h" +#include "tools/cabana/streams/abstractstream.h" using namespace QtCharts; +const int CHART_MIN_WIDTH = 300; + class ChartView : public QChartView { Q_OBJECT @@ -24,21 +30,19 @@ public: void addSeries(const QString &msg_id, const Signal *sig); void removeSeries(const QString &msg_id, const Signal *sig); bool hasSeries(const QString &msg_id, const Signal *sig) const; - void updateSeries(const Signal *sig = nullptr); - void setEventsRange(const std::pair &range); - void setDisplayRange(double min, double max); - void setPlotAreaLeftPosition(int pos); - qreal getYAsixLabelWidth() const; + void updateSeries(const Signal *sig = nullptr, const std::vector *events = nullptr, bool clear = true); + void updatePlot(double cur, double min, double max); + void setSeriesType(QAbstractSeries::SeriesType type); + void updatePlotArea(int left); struct SigItem { QString msg_id; uint8_t source = 0; uint32_t address = 0; const Signal *sig = nullptr; - QLineSeries *series = nullptr; - double min_y = 0; - double max_y = 0; + QXYSeries *series = nullptr; QVector vals; + uint64_t last_value_mono_time = 0; }; signals: @@ -47,7 +51,7 @@ signals: void zoomIn(double min, double max); void zoomReset(); void remove(); - void axisYUpdated(); + void axisYLabelWidthChanged(int w); private slots: void msgRemoved(uint32_t address); @@ -55,27 +59,42 @@ private slots: void signalUpdated(const Signal *sig); void signalRemoved(const Signal *sig); void manageSeries(); + void handleMarkerClicked(); private: - QList::iterator removeSeries(const QList::iterator &it); + QList::iterator removeItem(const QList::iterator &it); + void mousePressEvent(QMouseEvent *event) override; void mouseReleaseEvent(QMouseEvent *event) override; void mouseMoveEvent(QMouseEvent *ev) override; + void dragMoveEvent(QDragMoveEvent *event) override; + void dropEvent(QDropEvent *event) override; void leaveEvent(QEvent *event) override; void resizeEvent(QResizeEvent *event) override; + QSize sizeHint() const override { return {CHART_MIN_WIDTH, settings.chart_height}; } void updateAxisY(); void updateTitle(); - void updateFromSettings(); void drawForeground(QPainter *painter, const QRectF &rect) override; - void applyNiceNumbers(qreal min, qreal max); + std::tuple getNiceAxisNumbers(qreal min, qreal max, int tick_count); qreal niceNumber(qreal x, bool ceiling); + QXYSeries *createSeries(QAbstractSeries::SeriesType type, QColor color); + void updateSeriesPoints(); + int y_label_width = 0; + int align_to = 0; QValueAxis *axis_x; QValueAxis *axis_y; - QPointF track_pt; + QVector track_pts; + QGraphicsPixmapItem *move_icon; QGraphicsProxyWidget *close_btn_proxy; QGraphicsProxyWidget *manage_btn_proxy; - std::pair events_range = {0, 0}; + QGraphicsRectItem *background; QList sigs; + double cur_sec = 0; + const QString mime_type = "application/x-cabanachartview"; + QAbstractSeries::SeriesType series_type = QAbstractSeries::SeriesTypeLine; + QAction *line_series_action; + QAction *scatter_series_action; + friend class ChartsWidget; }; class ChartsWidget : public QWidget { @@ -86,57 +105,75 @@ public: void showChart(const QString &id, const Signal *sig, bool show, bool merge); inline bool hasSignal(const QString &id, const Signal *sig) { return findChart(id, sig) != nullptr; } +public slots: + void setColumnCount(int n); + void removeAll(); + signals: void dock(bool floating); void rangeChanged(double min, double max, bool is_zommed); void seriesChanged(); private: + void resizeEvent(QResizeEvent *event) override; void alignCharts(); + void newChart(); + ChartView * createChart(); void removeChart(ChartView *chart); void eventsMerged(); void updateState(); - void updateDisplayRange(); void zoomIn(double min, double max); void zoomReset(); void updateToolBar(); - void removeAll(); - void showAllData(); + void setMaxChartRange(int value); + void updateLayout(); + void settingChanged(); bool eventFilter(QObject *obj, QEvent *event) override; ChartView *findChart(const QString &id, const Signal *sig); QLabel *title_label; - QLabel *range_label; + QLabel *range_lb; + QSlider *range_slider; + QAction *range_lb_action; + QAction *range_slider_action; bool docking = true; - QAction *show_all_values_btn; QAction *dock_btn; - QAction *reset_zoom_btn; + QAction *reset_zoom_action; + QToolButton *reset_zoom_btn; QAction *remove_all_btn; - QTimer *align_charts_timer; - QVBoxLayout *charts_layout; + QGridLayout *charts_layout; QList charts; uint32_t max_chart_range = 0; bool is_zoomed = false; - std::pair event_range; std::pair display_range; std::pair zoomed_range; bool use_dark_theme = false; + QAction *columns_lb_action; + QAction *columns_cb_action; + QComboBox *columns_cb; + int column_count = 1; + int current_column_count = 0; }; class SeriesSelector : public QDialog { - Q_OBJECT - public: - SeriesSelector(QWidget *parent); - void addSeries(const QString &id, const QString& msg_name, const QString &sig_name); - QList series(); + struct ListItem : public QListWidgetItem { + ListItem(const QString &msg_id, const Signal *sig, QListWidget *parent) : msg_id(msg_id), sig(sig), QListWidgetItem(parent) {} + QString msg_id; + const Signal *sig; + }; -private slots: - void msgSelected(int index); - void addSignal(QListWidgetItem *item); + SeriesSelector(QString title, QWidget *parent); + QList seletedItems(); + inline void addSelected(const QString &id, const Signal *sig) { addItemToList(selected_list, id, sig, true); } private: + void updateAvailableList(int index); + void addItemToList(QListWidget *parent, const QString id, const Signal *sig, bool show_msg_name = false); + void add(QListWidgetItem *item); + void remove(QListWidgetItem *item); + QComboBox *msgs_combo; - QListWidget *sig_list; - QListWidget *chart_series; + QListWidget *available_list; + QListWidget *selected_list; }; diff --git a/tools/cabana/commands.cc b/tools/cabana/commands.cc index b3f5cb1c66..e4bf999062 100644 --- a/tools/cabana/commands.cc +++ b/tools/cabana/commands.cc @@ -1,3 +1,5 @@ +#include + #include "tools/cabana/commands.h" // EditMsgCommand @@ -73,3 +75,12 @@ EditSignalCommand::EditSignalCommand(const QString &id, const Signal *sig, const void EditSignalCommand::undo() { dbc()->updateSignal(id, new_signal.name.c_str(), old_signal); } void EditSignalCommand::redo() { dbc()->updateSignal(id, old_signal.name.c_str(), new_signal); } + +namespace UndoStack { + +QUndoStack *instance() { + static QUndoStack *undo_stack = new QUndoStack(qApp); + return undo_stack; +} + +} // namespace UndoStack diff --git a/tools/cabana/commands.h b/tools/cabana/commands.h index 7ea1f66653..c07a00b760 100644 --- a/tools/cabana/commands.h +++ b/tools/cabana/commands.h @@ -1,8 +1,8 @@ #pragma once #include +#include -#include "tools/cabana/canmessages.h" #include "tools/cabana/dbcmanager.h" class EditMsgCommand : public QUndoCommand { @@ -61,3 +61,8 @@ private: Signal old_signal = {}; Signal new_signal = {}; }; + +namespace UndoStack { + QUndoStack *instance(); + inline void push(QUndoCommand *cmd) { instance()->push(cmd); } +}; diff --git a/tools/cabana/dbcmanager.cc b/tools/cabana/dbcmanager.cc index 01bdff17a1..3d565e7067 100644 --- a/tools/cabana/dbcmanager.cc +++ b/tools/cabana/dbcmanager.cc @@ -13,10 +13,16 @@ void DBCManager::open(const QString &dbc_file_name) { initMsgMap(); } -void DBCManager::open(const QString &name, const QString &content) { - std::istringstream stream(content.toStdString()); - dbc = const_cast(dbc_parse_from_stream(name.toStdString(), stream)); - initMsgMap(); +bool DBCManager::open(const QString &name, const QString &content, QString *error) { + try { + std::istringstream stream(content.toStdString()); + dbc = const_cast(dbc_parse_from_stream(name.toStdString(), stream)); + initMsgMap(); + return true; + } catch (std::exception &e) { + if (error) *error = e.what(); + } + return false; } void DBCManager::initMsgMap() { @@ -68,7 +74,7 @@ void DBCManager::addSignal(const QString &id, const Signal &sig) { if (auto m = const_cast(msg(id))) { auto &s = m->sigs[sig.name.c_str()]; s = sig; - emit signalAdded(&s); + emit signalAdded(parseId(id).second, &s); } } @@ -110,6 +116,7 @@ DBCManager *dbc() { std::vector DBCMsg::getSignals() const { std::vector ret; + ret.reserve(sigs.size()); for (auto &[_, sig] : sigs) ret.push_back(&sig); std::sort(ret.begin(), ret.end(), [](auto l, auto r) { return l->start_bit < r->start_bit; }); return ret; diff --git a/tools/cabana/dbcmanager.h b/tools/cabana/dbcmanager.h index c7675121bb..41471a6169 100644 --- a/tools/cabana/dbcmanager.h +++ b/tools/cabana/dbcmanager.h @@ -8,7 +8,9 @@ struct DBCMsg { QString name; uint32_t size; + // signal must be saved as value in map to make undo stack work properly. std::map sigs; + // return vector, sort by start_bits std::vector getSignals() const; }; @@ -20,7 +22,7 @@ public: ~DBCManager(); void open(const QString &dbc_file_name); - void open(const QString &name, const QString &content); + bool open(const QString &name, const QString &content, QString *error = nullptr); QString generateDBC(); void addSignal(const QString &id, const Signal &sig); void updateSignal(const QString &id, const QString &sig_name, const Signal &sig); @@ -28,9 +30,7 @@ public: static std::pair parseId(const QString &id); inline static std::vector allDBCNames() { return get_dbc_names(); } - inline std::map &allMsgs() { return msgs; } inline QString name() const { return dbc ? dbc->name.c_str() : ""; } - void updateMsg(const QString &id, const QString &name, uint32_t size); void removeMsg(const QString &id); inline const std::map &messages() const { return msgs; } @@ -41,7 +41,7 @@ public: } signals: - void signalAdded(const Signal *sig); + void signalAdded(uint32_t address, const Signal *sig); void signalRemoved(const Signal *sig); void signalUpdated(const Signal *sig); void msgUpdated(uint32_t address); diff --git a/tools/cabana/detailwidget.cc b/tools/cabana/detailwidget.cc index 06377616da..25d9b3b9ca 100644 --- a/tools/cabana/detailwidget.cc +++ b/tools/cabana/detailwidget.cc @@ -4,23 +4,18 @@ #include #include #include -#include -#include +#include -#include "selfdrive/ui/qt/util.h" -#include "tools/cabana/canmessages.h" #include "tools/cabana/commands.h" #include "tools/cabana/dbcmanager.h" +#include "tools/cabana/streams/abstractstream.h" // DetailWidget DetailWidget::DetailWidget(ChartsWidget *charts, QWidget *parent) : charts(charts), QWidget(parent) { - undo_stack = new QUndoStack(this); - setMinimumWidth(500); QWidget *main_widget = new QWidget(this); QVBoxLayout *main_layout = new QVBoxLayout(main_widget); main_layout->setContentsMargins(0, 0, 0, 0); - main_layout->setSpacing(0); // tabbar tabbar = new QTabBar(this); @@ -30,76 +25,62 @@ DetailWidget::DetailWidget(ChartsWidget *charts, QWidget *parent) : charts(chart tabbar->setContextMenuPolicy(Qt::CustomContextMenu); main_layout->addWidget(tabbar); - QFrame *title_frame = new QFrame(this); - QVBoxLayout *frame_layout = new QVBoxLayout(title_frame); - title_frame->setFrameShape(QFrame::StyledPanel); - // message title - toolbar = new QToolBar(this); + QToolBar *toolbar = new QToolBar(this); + toolbar->setIconSize({16, 16}); toolbar->addWidget(new QLabel("time:")); time_label = new QLabel(this); time_label->setStyleSheet("font-weight:bold"); toolbar->addWidget(time_label); - name_label = new QLabel(this); + name_label = new ElidedLabel(this); + name_label->setContentsMargins(5, 0, 5, 0); name_label->setStyleSheet("font-weight:bold;"); name_label->setAlignment(Qt::AlignCenter); name_label->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Preferred); toolbar->addWidget(name_label); - toolbar->addAction("🖍", this, &DetailWidget::editMsg)->setToolTip(tr("Edit Message")); - remove_msg_act = toolbar->addAction("X", this, &DetailWidget::removeMsg); + toolbar->addAction(utils::icon("pencil"), "", this, &DetailWidget::editMsg)->setToolTip(tr("Edit Message")); + remove_msg_act = toolbar->addAction(utils::icon("x-lg"), "", this, &DetailWidget::removeMsg); remove_msg_act->setToolTip(tr("Remove Message")); - toolbar->setVisible(false); - frame_layout->addWidget(toolbar); + main_layout->addWidget(toolbar); // warning warning_widget = new QWidget(this); QHBoxLayout *warning_hlayout = new QHBoxLayout(warning_widget); - warning_hlayout->setContentsMargins(0, 0, 0, 0); - QLabel *warning_icon = new QLabel(this); - warning_icon->setPixmap(style()->standardPixmap(QStyle::SP_MessageBoxWarning).scaledToWidth(24, Qt::SmoothTransformation)); - warning_hlayout->addWidget(warning_icon, 0, Qt::AlignTop); - warning_label = new QLabel(this); - warning_hlayout->addWidget(warning_label, 1, Qt::AlignLeft); + warning_hlayout->addWidget(warning_icon = new QLabel(this), 0, Qt::AlignTop); + warning_hlayout->addWidget(warning_label = new QLabel(this), 1, Qt::AlignLeft); warning_widget->hide(); - frame_layout->addWidget(warning_widget); - main_layout->addWidget(title_frame); + main_layout->addWidget(warning_widget); // msg widget - QWidget *msg_widget = new QWidget(this); - QVBoxLayout *msg_layout = new QVBoxLayout(msg_widget); - msg_layout->setContentsMargins(0, 0, 0, 0); - // binary view - binary_view = new BinaryView(this); - msg_layout->addWidget(binary_view); - // signals - signals_layout = new QVBoxLayout(); - signals_layout->setSpacing(0); - msg_layout->addLayout(signals_layout); - msg_layout->addStretch(0); - - scroll = new QScrollArea(this); - scroll->setFrameShape(QFrame::NoFrame); - scroll->setWidget(msg_widget); - scroll->setWidgetResizable(true); - scroll->setHorizontalScrollBarPolicy(Qt::ScrollBarAlwaysOff); + splitter = new QSplitter(Qt::Vertical, this); + splitter->setAutoFillBackground(true); + splitter->addWidget(binary_view = new BinaryView(this)); + splitter->addWidget(signal_view = new SignalView(charts, this)); + binary_view->setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Minimum); + signal_view->setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Expanding); + splitter->setStretchFactor(0, 0); + splitter->setStretchFactor(1, 1); tab_widget = new QTabWidget(this); tab_widget->setTabPosition(QTabWidget::South); - tab_widget->addTab(scroll, "&Msg"); - history_log = new LogsWidget(this); - tab_widget->addTab(history_log, "&Logs"); + tab_widget->addTab(splitter, utils::icon("file-earmark-ruled"), "&Msg"); + tab_widget->addTab(history_log = new LogsWidget(this), utils::icon("stopwatch"), "&Logs"); main_layout->addWidget(tab_widget); stacked_layout = new QStackedLayout(this); stacked_layout->addWidget(new WelcomeWidget(this)); stacked_layout->addWidget(main_widget); - QObject::connect(binary_view, &BinaryView::signalClicked, this, &DetailWidget::showForm); - QObject::connect(binary_view, &BinaryView::resizeSignal, this, &DetailWidget::resizeSignal); - QObject::connect(binary_view, &BinaryView::addSignal, this, &DetailWidget::addSignal); + QObject::connect(binary_view, &BinaryView::resizeSignal, signal_view->model, &SignalModel::resizeSignal); + QObject::connect(binary_view, &BinaryView::addSignal, signal_view->model, &SignalModel::addSignal); + QObject::connect(binary_view, &BinaryView::signalHovered, signal_view, &SignalView::signalHovered); + QObject::connect(binary_view, &BinaryView::signalClicked, signal_view, &SignalView::expandSignal); + QObject::connect(signal_view, &SignalView::showChart, charts, &ChartsWidget::showChart); + QObject::connect(signal_view, &SignalView::highlight, binary_view, &BinaryView::highlight); QObject::connect(tab_widget, &QTabWidget::currentChanged, [this]() { updateState(); }); - QObject::connect(can, &CANMessages::msgsReceived, this, &DetailWidget::updateState); - QObject::connect(dbc(), &DBCManager::DBCFileChanged, [this]() { dbcMsgChanged(); }); + QObject::connect(can, &AbstractStream::msgsReceived, this, &DetailWidget::updateState); + QObject::connect(dbc(), &DBCManager::DBCFileChanged, this, &DetailWidget::refresh); + QObject::connect(UndoStack::instance(), &QUndoStack::indexChanged, this, &DetailWidget::refresh); QObject::connect(tabbar, &QTabBar::customContextMenuRequested, this, &DetailWidget::showTabBarContextMenu); QObject::connect(tabbar, &QTabBar::currentChanged, [this](int index) { if (index != -1 && tabbar->tabText(index) != msg_id) { @@ -107,14 +88,7 @@ DetailWidget::DetailWidget(ChartsWidget *charts, QWidget *parent) : charts(chart } }); QObject::connect(tabbar, &QTabBar::tabCloseRequested, tabbar, &QTabBar::removeTab); - QObject::connect(charts, &ChartsWidget::seriesChanged, this, &DetailWidget::updateChartState); - QObject::connect(history_log, &LogsWidget::openChart, [this](const QString &id, const Signal *sig) { - this->charts->showChart(id, sig, true, false); - }); - QObject::connect(undo_stack, &QUndoStack::indexChanged, [this]() { - if (undo_stack->count() > 0) - dbcMsgChanged(); - }); + QObject::connect(charts, &ChartsWidget::seriesChanged, signal_view, &SignalView::updateChartState); } void DetailWidget::showTabBarContextMenu(const QPoint &pt) { @@ -131,6 +105,16 @@ void DetailWidget::showTabBarContextMenu(const QPoint &pt) { } } +void DetailWidget::removeAll() { + msg_id = ""; + tabbar->blockSignals(true); + while (tabbar->count() > 0) { + tabbar->removeTab(0); + } + tabbar->blockSignals(false); + stacked_layout->setCurrentIndex(0); +} + void DetailWidget::setMessage(const QString &message_id) { msg_id = message_id; int index = tabbar->count() - 1; @@ -139,60 +123,47 @@ void DetailWidget::setMessage(const QString &message_id) { index = tabbar->addTab(message_id); tabbar->setTabToolTip(index, msgName(message_id)); } - tabbar->setCurrentIndex(index); - dbcMsgChanged(); - scroll->verticalScrollBar()->setValue(0); - stacked_layout->setCurrentIndex(1); -} - -void DetailWidget::dbcMsgChanged(int show_form_idx) { - if (msg_id.isEmpty()) return; setUpdatesEnabled(false); + signal_view->setMessage(msg_id); binary_view->setMessage(msg_id); history_log->setMessage(msg_id); - int i = 0; + stacked_layout->setCurrentIndex(1); + tabbar->setCurrentIndex(index); + refresh(); + splitter->setSizes({1, 2}); + + setUpdatesEnabled(true); +} + +void DetailWidget::refresh() { + if (msg_id.isEmpty()) return; + QStringList warnings; const DBCMsg *msg = dbc()->msg(msg_id); if (msg) { - for (auto sig : msg->getSignals()) { - SignalEdit *form = i < signal_list.size() ? signal_list[i] : nullptr; - if (!form) { - form = new SignalEdit(i, this); - QObject::connect(form, &SignalEdit::remove, this, &DetailWidget::removeSignal); - QObject::connect(form, &SignalEdit::save, this, &DetailWidget::saveSignal); - QObject::connect(form, &SignalEdit::showFormClicked, this, &DetailWidget::showForm); - QObject::connect(form, &SignalEdit::highlight, binary_view, &BinaryView::highlight); - QObject::connect(binary_view, &BinaryView::signalHovered, form, &SignalEdit::signalHovered); - QObject::connect(form, &SignalEdit::showChart, charts, &ChartsWidget::showChart); - signals_layout->addWidget(form); - signal_list.push_back(form); - } - form->setSignal(msg_id, sig); - form->setChartOpened(charts->hasSignal(msg_id, sig)); - ++i; - } - if (msg->size != can->lastMessage(msg_id).dat.size()) + if (msg->size != can->lastMessage(msg_id).dat.size()) { warnings.push_back(tr("Message size (%1) is incorrect.").arg(msg->size)); + } + for (auto s : binary_view->getOverlappingSignals()) { + warnings.push_back(tr("%1 has overlapping bits.").arg(s->name.c_str())); + } + } else { + warnings.push_back(tr("Drag-Select in binary view to create new signal.")); } - for (/**/; i < signal_list.size(); ++i) - signal_list[i]->hide(); - - toolbar->setVisible(!msg_id.isEmpty()); remove_msg_act->setEnabled(msg != nullptr); name_label->setText(msgName(msg_id)); - for (auto s : binary_view->getOverlappingSignals()) - warnings.push_back(tr("%1 has overlapping bits.").arg(s->name.c_str())); - - warning_label->setText(warnings.join('\n')); + if (!warnings.isEmpty()) { + warning_label->setText(warnings.join('\n')); + warning_icon->setPixmap(utils::icon(msg ? "exclamation-triangle" : "info-circle")); + } warning_widget->setVisible(!warnings.isEmpty()); - setUpdatesEnabled(true); } -void DetailWidget::updateState(const QHash * msgs) { +void DetailWidget::updateState(const QHash *msgs) { time_label->setText(QString::number(can->currentSec(), 'f', 3)); if (msg_id.isEmpty() || (msgs && !msgs->contains(msg_id))) return; @@ -203,86 +174,18 @@ void DetailWidget::updateState(const QHash * msgs) { history_log->updateState(); } -void DetailWidget::showForm(const Signal *sig) { - setUpdatesEnabled(false); - for (auto f : signal_list) { - f->updateForm(f->sig == sig && !f->form->isVisible()); - if (f->sig == sig && f->form->isVisible()) { - QTimer::singleShot(0, [=]() { scroll->ensureWidgetVisible(f); }); - } - } - setUpdatesEnabled(true); -} - -void DetailWidget::updateChartState() { - for (auto f : signal_list) - f->setChartOpened(charts->hasSignal(f->msg_id, f->sig)); -} - void DetailWidget::editMsg() { QString id = msg_id; auto msg = dbc()->msg(id); int size = msg ? msg->size : can->lastMessage(id).dat.size(); EditMessageDialog dlg(id, msgName(id), size, this); if (dlg.exec()) { - undo_stack->push(new EditMsgCommand(msg_id, dlg.name_edit->text(), dlg.size_spin->value())); + UndoStack::push(new EditMsgCommand(msg_id, dlg.name_edit->text(), dlg.size_spin->value())); } } void DetailWidget::removeMsg() { - undo_stack->push(new RemoveMsgCommand(msg_id)); -} - -void DetailWidget::addSignal(int start_bit, int size, bool little_endian) { - auto msg = dbc()->msg(msg_id); - if (!msg) { - for (int i = 1; /**/; ++i) { - QString name = QString("NEW_MSG_%1").arg(i); - auto it = std::find_if(dbc()->messages().begin(), dbc()->messages().end(), [&](auto &m) { return m.second.name == name; }); - if (it == dbc()->messages().end()) { - undo_stack->push(new EditMsgCommand(msg_id, name, can->lastMessage(msg_id).dat.size())); - msg = dbc()->msg(msg_id); - break; - } - } - } - Signal sig = {.is_little_endian = little_endian, .factor = 1}; - for (int i = 1; /**/; ++i) { - sig.name = "NEW_SIGNAL_" + std::to_string(i); - if (msg->sigs.count(sig.name.c_str()) == 0) break; - } - updateSigSizeParamsFromRange(sig, start_bit, size); - undo_stack->push(new AddSigCommand(msg_id, sig)); -} - -void DetailWidget::resizeSignal(const Signal *sig, int start_bit, int size) { - Signal s = *sig; - updateSigSizeParamsFromRange(s, start_bit, size); - saveSignal(sig, s); -} - -void DetailWidget::saveSignal(const Signal *sig, const Signal &new_sig) { - auto msg = dbc()->msg(msg_id); - if (new_sig.name != sig->name) { - auto it = msg->sigs.find(new_sig.name.c_str()); - if (it != msg->sigs.end()) { - QString warning_str = tr("There is already a signal with the same name '%1'").arg(new_sig.name.c_str()); - QMessageBox::warning(this, tr("Failed to save signal"), warning_str); - return; - } - } - auto [start, end] = getSignalRange(&new_sig); - if (start < 0 || end >= msg->size * 8) { - QString warning_str = tr("Signal size [%1] exceed limit").arg(new_sig.size); - QMessageBox::warning(this, tr("Failed to save signal"), warning_str); - return; - } - - undo_stack->push(new EditSignalCommand(msg_id, sig, new_sig)); -} - -void DetailWidget::removeSignal(const Signal *sig) { - undo_stack->push(new RemoveSigCommand(msg_id, sig)); + UndoStack::push(new RemoveMsgCommand(msg_id)); } // EditMessageDialog @@ -293,7 +196,7 @@ EditMessageDialog::EditMessageDialog(const QString &msg_id, const QString &title form_layout->addRow("ID", new QLabel(msg_id)); name_edit = new QLineEdit(title, this); - name_edit->setValidator(new QRegExpValidator(QRegExp("^(\\w+)"), name_edit)); + name_edit->setValidator(new NameValidator(name_edit)); form_layout->addRow(tr("Name"), name_edit); size_spin = new QSpinBox(this); @@ -330,9 +233,14 @@ WelcomeWidget::WelcomeWidget(QWidget *parent) : QWidget(parent) { return hlayout; }; + auto lb = new QLabel(tr("<-Select a message to to view details")); + lb->setAlignment(Qt::AlignHCenter); + main_layout->addWidget(lb); main_layout->addLayout(newShortcutRow("Pause", "Space")); main_layout->addLayout(newShortcutRow("Help", "Alt + H")); main_layout->addStretch(0); setStyleSheet("QLabel{color:darkGray;}"); + setBackgroundRole(QPalette::Base); + setAutoFillBackground(true); } diff --git a/tools/cabana/detailwidget.h b/tools/cabana/detailwidget.h index 18c58ab6bf..0983f49924 100644 --- a/tools/cabana/detailwidget.h +++ b/tools/cabana/detailwidget.h @@ -1,11 +1,11 @@ #pragma once +#include #include -#include #include #include -#include +#include "selfdrive/ui/qt/widgets/controls.h" #include "tools/cabana/binaryview.h" #include "tools/cabana/chartswidget.h" #include "tools/cabana/historylog.h" @@ -30,33 +30,27 @@ class DetailWidget : public QWidget { public: DetailWidget(ChartsWidget *charts, QWidget *parent); void setMessage(const QString &message_id); - void dbcMsgChanged(int show_form_idx = -1); - QUndoStack *undo_stack = nullptr; + void refresh(); + void removeAll(); + QSize minimumSizeHint() const override { return binary_view->minimumSizeHint(); } private: - void showForm(const Signal *sig); - void updateChartState(); void showTabBarContextMenu(const QPoint &pt); - void addSignal(int start_bit, int size, bool little_endian); - void resizeSignal(const Signal *sig, int from, int to); - void saveSignal(const Signal *sig, const Signal &new_sig); - void removeSignal(const Signal *sig); void editMsg(); void removeMsg(); void updateState(const QHash * msgs = nullptr); QString msg_id; - QLabel *name_label, *time_label, *warning_label; + QLabel *time_label, *warning_icon, *warning_label; + ElidedLabel *name_label; QWidget *warning_widget; - QVBoxLayout *signals_layout; QTabBar *tabbar; QTabWidget *tab_widget; - QToolBar *toolbar; QAction *remove_msg_act; LogsWidget *history_log; BinaryView *binary_view; - QScrollArea *scroll; + SignalView *signal_view; ChartsWidget *charts; + QSplitter *splitter; QStackedLayout *stacked_layout; - QList signal_list; }; diff --git a/tools/cabana/historylog.cc b/tools/cabana/historylog.cc index 485a21cc1b..e4ad99758b 100644 --- a/tools/cabana/historylog.cc +++ b/tools/cabana/historylog.cc @@ -1,56 +1,55 @@ #include "tools/cabana/historylog.h" -#include #include #include #include +#include "tools/cabana/commands.h" + // HistoryLogModel QVariant HistoryLogModel::data(const QModelIndex &index, int role) const { + const bool show_signals = display_signals_mode && sigs.size() > 0; + const auto &m = messages[index.row()]; if (role == Qt::DisplayRole) { - const auto &m = messages[index.row()]; if (index.column() == 0) { return QString::number((m.mono_time / (double)1e9) - can->routeStartTime(), 'f', 2); } - return !sigs.empty() ? QString::number(m.sig_values[index.column() - 1]) : m.data; - } else if (role == Qt::FontRole && index.column() == 1 && sigs.empty()) { - return QFontDatabase::systemFont(QFontDatabase::FixedFont); - } else if (role == Qt::ToolTipRole && index.column() > 0 && !sigs.empty()) { - return tr("double click to open the chart"); + return show_signals ? QString::number(m.sig_values[index.column() - 1]) : toHex(m.data); + } else if (role == Qt::UserRole && index.column() == 1 && !show_signals) { + return ChangeTracker::toVariantList(m.colors); } return {}; } void HistoryLogModel::setMessage(const QString &message_id) { msg_id = message_id; - sigs.clear(); - if (auto dbc_msg = dbc()->msg(msg_id)) { - sigs = dbc_msg->getSignals(); - } - filter_cmp = nullptr; - refresh(); } void HistoryLogModel::refresh() { beginResetModel(); + sigs.clear(); + if (auto dbc_msg = dbc()->msg(msg_id)) { + sigs = dbc_msg->getSignals(); + } last_fetch_time = 0; + has_more_data = true; messages.clear(); + hex_colors.clear(); updateState(); endResetModel(); } QVariant HistoryLogModel::headerData(int section, Qt::Orientation orientation, int role) const { if (orientation == Qt::Horizontal) { + const bool show_signals = display_signals_mode && !sigs.empty(); if (role == Qt::DisplayRole || role == Qt::ToolTipRole) { if (section == 0) { return "Time"; } - return !sigs.empty() ? QString::fromStdString(sigs[section - 1]->name).replace('_', ' ') : "Data"; - } else if (role == Qt::BackgroundRole && section > 0 && !sigs.empty()) { - return QBrush(QColor(getColor(section - 1))); - } else if (role == Qt::ForegroundRole && section > 0 && !sigs.empty()) { - return QBrush(Qt::black); + return show_signals ? QString::fromStdString(sigs[section - 1]->name).replace('_', ' ') : "Data"; + } else if (role == Qt::BackgroundRole && section > 0 && show_signals) { + return QBrush(getColor(sigs[section - 1])); } } return {}; @@ -61,6 +60,11 @@ void HistoryLogModel::setDynamicMode(int state) { refresh(); } +void HistoryLogModel::setDisplayType(int type) { + display_signals_mode = type == 0; + refresh(); +} + void HistoryLogModel::segmentsMerged() { if (!dynamic_mode) { has_more_data = true; @@ -71,18 +75,18 @@ void HistoryLogModel::setFilter(int sig_idx, const QString &value, std::function filter_sig_idx = sig_idx; filter_value = value.toDouble(); filter_cmp = value.isEmpty() ? nullptr : cmp; - refresh(); } void HistoryLogModel::updateState() { if (!msg_id.isEmpty()) { - uint64_t current_time = (can->currentSec() + can->routeStartTime()) * 1e9; + uint64_t current_time = (can->lastMessage(msg_id).ts + can->routeStartTime()) * 1e9 + 1; auto new_msgs = dynamic_mode ? fetchData(current_time, last_fetch_time) : fetchData(0); - if ((has_more_data = !new_msgs.empty())) { + if (!new_msgs.empty()) { beginInsertRows({}, 0, new_msgs.size() - 1); messages.insert(messages.begin(), std::move_iterator(new_msgs.begin()), std::move_iterator(new_msgs.end())); endInsertRows(); } + has_more_data = new_msgs.size() >= batch_size; last_fetch_time = current_time; } } @@ -90,11 +94,12 @@ void HistoryLogModel::updateState() { void HistoryLogModel::fetchMore(const QModelIndex &parent) { if (!messages.empty()) { auto new_msgs = fetchData(messages.back().mono_time); - if ((has_more_data = !new_msgs.empty())) { + if (!new_msgs.empty()) { beginInsertRows({}, messages.size(), messages.size() + new_msgs.size() - 1); messages.insert(messages.end(), std::move_iterator(new_msgs.begin()), std::move_iterator(new_msgs.end())); endInsertRows(); } + has_more_data = new_msgs.size() >= batch_size; } } @@ -106,7 +111,7 @@ std::deque HistoryLogModel::fetchData(InputIt first, I for (auto it = first; it != last && (*it)->mono_time > min_time; ++it) { if ((*it)->which == cereal::Event::Which::CAN) { for (const auto &c : (*it)->event.getCan()) { - if (src == c.getSrc() && address == c.getAddress()) { + if (address == c.getAddress() && src == c.getSrc()) { const auto dat = c.getDat(); for (int i = 0; i < sigs.size(); ++i) { values[i] = get_raw_value((uint8_t *)dat.begin(), dat.size(), *(sigs[i])); @@ -114,7 +119,7 @@ std::deque HistoryLogModel::fetchData(InputIt first, I if (!filter_cmp || filter_cmp(values[filter_sig_idx], filter_value)) { auto &m = msgs.emplace_back(); m.mono_time = (*it)->mono_time; - m.data = toHex(QByteArray((char *)dat.begin(), dat.size())); + m.data = QByteArray((char *)dat.begin(), dat.size()); m.sig_values = values; if (msgs.size() >= batch_size && min_time == 0) return msgs; @@ -125,143 +130,149 @@ std::deque HistoryLogModel::fetchData(InputIt first, I } return msgs; } + template std::deque HistoryLogModel::fetchData<>(std::vector::iterator first, std::vector::iterator last, uint64_t min_time); template std::deque HistoryLogModel::fetchData<>(std::vector::reverse_iterator first, std::vector::reverse_iterator last, uint64_t min_time); std::deque HistoryLogModel::fetchData(uint64_t from_time, uint64_t min_time) { auto events = can->events(); + const auto freq = can->lastMessage(msg_id).freq; + const bool update_colors = !display_signals_mode || sigs.empty(); + if (dynamic_mode) { - auto it = std::lower_bound(events->rbegin(), events->rend(), from_time, [=](auto &e, uint64_t ts) { - return e->mono_time > ts; - }); - if (it != events->rend()) ++it; - return fetchData(it, events->rend(), min_time); + auto first = std::upper_bound(events->rbegin(), events->rend(), from_time, [=](uint64_t ts, auto &e) { return e->mono_time < ts; }); + auto msgs = fetchData(first, events->rend(), min_time); + if (update_colors && (min_time > 0 || messages.empty())) { + for (auto it = msgs.rbegin(); it != msgs.rend(); ++it) { + hex_colors.compute(it->data, it->mono_time / (double)1e9, freq); + it->colors = hex_colors.colors; + } + } + return msgs; } else { assert(min_time == 0); - auto it = std::upper_bound(events->begin(), events->end(), from_time, [=](uint64_t ts, auto &e) { - return ts < e->mono_time; - }); - return fetchData(it, events->end(), 0); + auto first = std::upper_bound(events->begin(), events->end(), from_time, [=](uint64_t ts, auto &e) { return ts < e->mono_time; }); + auto msgs = fetchData(first, events->end(), 0); + if (update_colors) { + for (auto it = msgs.rbegin(); it != msgs.rend(); ++it) { + hex_colors.compute(it->data, it->mono_time / (double)1e9, freq); + it->colors = hex_colors.colors; + } + } + return msgs; } } // HeaderView QSize HeaderView::sectionSizeFromContents(int logicalIndex) const { - int default_size = qMax(100, rect().width() / model()->columnCount()); - const QString text = model()->headerData(logicalIndex, this->orientation(), Qt::DisplayRole).toString(); - const QRect rect = fontMetrics().boundingRect({0, 0, default_size, 2000}, defaultAlignment(), text); - QSize size = rect.size() + QSize{10, 6}; - return {qMax(size.width(), default_size), size.height()}; + static QSize time_col_size = fontMetrics().boundingRect({0, 0, 200, 200}, defaultAlignment(), "000000.000").size() + QSize(10, 6); + if (logicalIndex == 0) { + return time_col_size; + } else { + int default_size = qMax(100, (rect().width() - time_col_size.width()) / (model()->columnCount() - 1)); + const QString text = model()->headerData(logicalIndex, this->orientation(), Qt::DisplayRole).toString(); + const QRect rect = fontMetrics().boundingRect({0, 0, default_size, 2000}, defaultAlignment(), text); + QSize size = rect.size() + QSize{10, 6}; + return QSize{qMax(size.width(), default_size), size.height()}; + } } void HeaderView::paintSection(QPainter *painter, const QRect &rect, int logicalIndex) const { auto bg_role = model()->headerData(logicalIndex, Qt::Horizontal, Qt::BackgroundRole); if (bg_role.isValid()) { - QPen pen(model()->headerData(logicalIndex, Qt::Horizontal, Qt::ForegroundRole).value(), 1); - painter->setPen(pen); painter->fillRect(rect, bg_role.value()); } QString text = model()->headerData(logicalIndex, Qt::Horizontal, Qt::DisplayRole).toString(); painter->drawText(rect.adjusted(5, 3, -5, -3), defaultAlignment(), text); } -// HistoryLog - -HistoryLog::HistoryLog(QWidget *parent) : QTableView(parent) { - setHorizontalHeader(new HeaderView(Qt::Horizontal, this)); - horizontalHeader()->setDefaultAlignment(Qt::AlignLeft | (Qt::Alignment)Qt::TextWordWrap); - horizontalHeader()->setSectionResizeMode(QHeaderView::ResizeToContents); - verticalHeader()->setVisible(false); - setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Expanding); -} - // LogsWidget LogsWidget::LogsWidget(QWidget *parent) : QWidget(parent) { QVBoxLayout *main_layout = new QVBoxLayout(this); + main_layout->setContentsMargins(0, 0, 0, 0); + main_layout->setSpacing(0); + + QWidget *toolbar = new QWidget(this); + toolbar->setAutoFillBackground(true); + QHBoxLayout *h = new QHBoxLayout(toolbar); + + filters_widget = new QWidget(this); + QHBoxLayout *filter_layout = new QHBoxLayout(filters_widget); + filter_layout->setContentsMargins(0, 0, 0, 0); + filter_layout->addWidget(display_type_cb = new QComboBox(this)); + filter_layout->addWidget(signals_cb = new QComboBox(this)); + filter_layout->addWidget(comp_box = new QComboBox(this)); + filter_layout->addWidget(value_edit = new QLineEdit(this)); + h->addWidget(filters_widget); + h->addStretch(0); + h->addWidget(dynamic_mode = new QCheckBox(tr("Dynamic")), 0, Qt::AlignRight); - QHBoxLayout *h = new QHBoxLayout(); - signals_cb = new QComboBox(this); - signals_cb->setSizePolicy(QSizePolicy::MinimumExpanding, QSizePolicy::Preferred); - h->addWidget(signals_cb); - comp_box = new QComboBox(); + display_type_cb->addItems({"Signal Value", "Hex Value"}); comp_box->addItems({">", "=", "!=", "<"}); - h->addWidget(comp_box); - value_edit = new QLineEdit(this); value_edit->setClearButtonEnabled(true); value_edit->setValidator(new QDoubleValidator(-500000, 500000, 6, this)); - h->addWidget(value_edit); - dynamic_mode = new QCheckBox(tr("Dynamic")); - h->addWidget(dynamic_mode, 0, Qt::AlignRight); - main_layout->addLayout(h); + dynamic_mode->setChecked(true); + dynamic_mode->setEnabled(!can->liveStreaming()); - model = new HistoryLogModel(this); - logs = new HistoryLog(this); - logs->setModel(model); - main_layout->addWidget(logs); + main_layout->addWidget(toolbar); + QFrame *line = new QFrame(this); + line->setFrameStyle(QFrame::HLine | QFrame::Sunken); + main_layout->addWidget(line);; - QObject::connect(logs, &QTableView::doubleClicked, this, &LogsWidget::doubleClicked); + main_layout->addWidget(logs = new QTableView(this)); + logs->setModel(model = new HistoryLogModel(this)); + logs->setItemDelegateForColumn(1, new MessageBytesDelegate(this)); + logs->setHorizontalHeader(new HeaderView(Qt::Horizontal, this)); + logs->horizontalHeader()->setDefaultAlignment(Qt::AlignLeft | (Qt::Alignment)Qt::TextWordWrap); + logs->horizontalHeader()->setSectionResizeMode(QHeaderView::ResizeToContents); + logs->verticalHeader()->setVisible(false); + logs->setFrameShape(QFrame::NoFrame); + + QObject::connect(display_type_cb, SIGNAL(activated(int)), model, SLOT(setDisplayType(int))); + QObject::connect(dynamic_mode, &QCheckBox::stateChanged, model, &HistoryLogModel::setDynamicMode); QObject::connect(signals_cb, SIGNAL(activated(int)), this, SLOT(setFilter())); QObject::connect(comp_box, SIGNAL(activated(int)), this, SLOT(setFilter())); QObject::connect(value_edit, &QLineEdit::textChanged, this, &LogsWidget::setFilter); - QObject::connect(dynamic_mode, &QCheckBox::stateChanged, model, &HistoryLogModel::setDynamicMode); - QObject::connect(can, &CANMessages::seekedTo, model, &HistoryLogModel::refresh); - QObject::connect(can, &CANMessages::eventsMerged, model, &HistoryLogModel::segmentsMerged); + QObject::connect(can, &AbstractStream::seekedTo, model, &HistoryLogModel::refresh); + QObject::connect(dbc(), &DBCManager::DBCFileChanged, this, &LogsWidget::refresh); + QObject::connect(UndoStack::instance(), &QUndoStack::indexChanged, this, &LogsWidget::refresh); + QObject::connect(can, &AbstractStream::eventsMerged, model, &HistoryLogModel::segmentsMerged); } void LogsWidget::setMessage(const QString &message_id) { model->setMessage(message_id); - cur_filter_text = ""; - value_edit->setText(""); - signals_cb->clear(); - comp_box->setCurrentIndex(0); - bool has_signals = model->sigs.size() > 0; - if (has_signals) { + refresh(); +} + +void LogsWidget::refresh() { + if (model->msg_id.isEmpty()) return; + + model->setFilter(0, "", nullptr); + model->refresh(); + bool has_signal = model->sigs.size(); + if (has_signal) { + signals_cb->clear(); for (auto s : model->sigs) { signals_cb->addItem(s->name.c_str()); } } - comp_box->setVisible(has_signals); - value_edit->setVisible(has_signals); - signals_cb->setVisible(has_signals); + value_edit->clear(); + comp_box->setCurrentIndex(0); + filters_widget->setVisible(has_signal); } -static bool not_equal(double l, double r) { return l != r; } - void LogsWidget::setFilter() { - if (cur_filter_text.isEmpty() && value_edit->text().isEmpty()) { - return; - } + if (value_edit->text().isEmpty() && !value_edit->isModified()) return; - std::function cmp; + std::function cmp = nullptr; switch (comp_box->currentIndex()) { case 0: cmp = std::greater{}; break; case 1: cmp = std::equal_to{}; break; - case 2: cmp = not_equal; break; + case 2: cmp = [](double l, double r) { return l != r; }; break; // not equal case 3: cmp = std::less{}; break; } model->setFilter(signals_cb->currentIndex(), value_edit->text(), cmp); - cur_filter_text = value_edit->text(); -} - -void LogsWidget::showEvent(QShowEvent *event) { - if (dynamic_mode->isChecked()) { - model->refresh(); - } -} - -void LogsWidget::updateState() { - if (dynamic_mode->isChecked()) { - model->updateState(); - } -} - -void LogsWidget::doubleClicked(const QModelIndex &index) { - if (index.isValid()) { - if (model->sigs.size() > 0 && index.column() > 0) { - emit openChart(model->msg_id, model->sigs[index.column()-1]); - } - can->seekTo(model->messages[index.row()].mono_time / (double)1e9 - can->routeStartTime()); - } + model->refresh(); } diff --git a/tools/cabana/historylog.h b/tools/cabana/historylog.h index f20f51637a..2458fc1c31 100644 --- a/tools/cabana/historylog.h +++ b/tools/cabana/historylog.h @@ -7,8 +7,8 @@ #include #include -#include "tools/cabana/canmessages.h" #include "tools/cabana/dbcmanager.h" +#include "tools/cabana/streams/abstractstream.h" class HeaderView : public QHeaderView { public: @@ -30,15 +30,22 @@ public: void fetchMore(const QModelIndex &parent) override; inline bool canFetchMore(const QModelIndex &parent) const override { return has_more_data; } int rowCount(const QModelIndex &parent = QModelIndex()) const override { return messages.size(); } - int columnCount(const QModelIndex &parent = QModelIndex()) const override { return std::max(1ul, sigs.size()) + 1; } + int columnCount(const QModelIndex &parent = QModelIndex()) const override { + return display_signals_mode && !sigs.empty() ? sigs.size() + 1 : 2; + } + void refresh(); + +public slots: + void setDisplayType(int type); void setDynamicMode(int state); void segmentsMerged(); - void refresh(); +public: struct Message { uint64_t mono_time = 0; QVector sig_values; - QString data; + QByteArray data; + QVector colors; }; template @@ -46,6 +53,7 @@ public: std::deque fetchData(uint64_t from_time, uint64_t min_time = 0); QString msg_id; + ChangeTracker hex_colors; bool has_more_data = true; const int batch_size = 50; int filter_sig_idx = -1; @@ -54,13 +62,8 @@ public: std::function filter_cmp = nullptr; std::deque messages; std::vector sigs; - bool dynamic_mode = false; -}; - -class HistoryLog : public QTableView { -public: - HistoryLog(QWidget *parent); - int sizeHintForColumn(int column) const override { return -1; }; + bool dynamic_mode = true; + bool display_signals_mode = true; }; class LogsWidget : public QWidget { @@ -69,22 +72,19 @@ class LogsWidget : public QWidget { public: LogsWidget(QWidget *parent); void setMessage(const QString &message_id); - void updateState(); - -signals: - void openChart(const QString &msg_id, const Signal *sig); + void updateState() {if (dynamic_mode->isChecked()) model->updateState(); } + void showEvent(QShowEvent *event) override { if (dynamic_mode->isChecked()) model->refresh(); } private slots: void setFilter(); private: - void doubleClicked(const QModelIndex &index); - void showEvent(QShowEvent *event) override; + void refresh(); - HistoryLog *logs; + QTableView *logs; HistoryLogModel *model; QCheckBox *dynamic_mode; - QComboBox *signals_cb, *comp_box; + QComboBox *signals_cb, *comp_box, *display_type_cb; QLineEdit *value_edit; - QString cur_filter_text; + QWidget *filters_widget; }; diff --git a/tools/cabana/mainwin.cc b/tools/cabana/mainwin.cc index 9df5894b90..bce6d313c8 100644 --- a/tools/cabana/mainwin.cc +++ b/tools/cabana/mainwin.cc @@ -2,7 +2,6 @@ #include #include -#include #include #include #include @@ -15,6 +14,9 @@ #include #include +#include "tools/cabana/commands.h" +#include "tools/cabana/route.h" + static MainWindow *main_win = nullptr; void qLogMessageHandler(QtMsgType type, const QMessageLogContext &context, const QString &msg) { if (type == QtDebugMsg) std::cout << msg.toStdString() << std::endl; @@ -24,17 +26,18 @@ void qLogMessageHandler(QtMsgType type, const QMessageLogContext &context, const MainWindow::MainWindow() : QMainWindow() { createDockWindows(); detail_widget = new DetailWidget(charts_widget, this); - detail_widget->setSizePolicy(QSizePolicy::Minimum, QSizePolicy::Preferred); setCentralWidget(detail_widget); createActions(); createStatusBar(); createShortcuts(); + // restore states restoreGeometry(settings.geometry); if (isMaximized()) { setGeometry(QApplication::desktop()->availableGeometry(this)); } restoreState(settings.window_state); + messages_widget->restoreHeaderState(settings.message_header_state); qRegisterMetaType("uint64_t"); qRegisterMetaType("ReplyMsgType"); @@ -53,40 +56,66 @@ MainWindow::MainWindow() : QMainWindow() { fingerprint_to_dbc = QJsonDocument::fromJson(json_file.readAll()); } - QObject::connect(dbc_combo, SIGNAL(activated(const QString &)), SLOT(loadDBCFromName(const QString &))); QObject::connect(this, &MainWindow::showMessage, statusBar(), &QStatusBar::showMessage); QObject::connect(this, &MainWindow::updateProgressBar, this, &MainWindow::updateDownloadProgress); QObject::connect(messages_widget, &MessagesWidget::msgSelectionChanged, detail_widget, &DetailWidget::setMessage); QObject::connect(charts_widget, &ChartsWidget::dock, this, &MainWindow::dockCharts); - QObject::connect(charts_widget, &ChartsWidget::rangeChanged, video_widget, &VideoWidget::rangeChanged); - QObject::connect(can, &CANMessages::streamStarted, this, &MainWindow::loadDBCFromFingerprint); + QObject::connect(can, &AbstractStream::streamStarted, this, &MainWindow::loadDBCFromFingerprint); QObject::connect(dbc(), &DBCManager::DBCFileChanged, this, &MainWindow::DBCFileChanged); - QObject::connect(detail_widget->undo_stack, &QUndoStack::indexChanged, [this](int index) { - setWindowTitle(tr("%1%2 - Cabana").arg(index > 0 ? "* " : "").arg(dbc()->name())); - }); + QObject::connect(UndoStack::instance(), &QUndoStack::cleanChanged, this, &MainWindow::undoStackCleanChanged); } void MainWindow::createActions() { QMenu *file_menu = menuBar()->addMenu(tr("&File")); - file_menu->addAction(tr("Open DBC File..."), this, &MainWindow::loadDBCFromFile); + if (!can->liveStreaming()) { + file_menu->addAction(tr("Open Route..."), this, &MainWindow::openRoute); + file_menu->addSeparator(); + } + + file_menu->addAction(tr("New DBC File"), this, &MainWindow::newFile)->setShortcuts(QKeySequence::New); + file_menu->addAction(tr("Open DBC File..."), this, &MainWindow::openFile)->setShortcuts(QKeySequence::Open); + + open_recent_menu = file_menu->addMenu(tr("Open &Recent")); + for (int i = 0; i < MAX_RECENT_FILES; ++i) { + recent_files_acts[i] = new QAction(this); + recent_files_acts[i]->setVisible(false); + QObject::connect(recent_files_acts[i], &QAction::triggered, this, &MainWindow::openRecentFile); + open_recent_menu->addAction(recent_files_acts[i]); + } + updateRecentFileActions(); + + file_menu->addSeparator(); + QMenu *load_opendbc_menu = file_menu->addMenu(tr("Load DBC from commaai/opendbc")); + // load_opendbc_menu->setStyleSheet("QMenu { menu-scrollable: true; }"); + auto dbc_names = dbc()->allDBCNames(); + std::sort(dbc_names.begin(), dbc_names.end()); + for (const auto &name : dbc_names) { + load_opendbc_menu->addAction(QString::fromStdString(name), this, &MainWindow::openOpendbcFile); + } + file_menu->addAction(tr("Load DBC From Clipboard"), this, &MainWindow::loadDBCFromClipboard); + file_menu->addSeparator(); - file_menu->addAction(tr("Save DBC As..."), this, &MainWindow::saveDBCToFile); + file_menu->addAction(tr("Save DBC..."), this, &MainWindow::save)->setShortcuts(QKeySequence::Save); + file_menu->addAction(tr("Save DBC As..."), this, &MainWindow::saveAs)->setShortcuts(QKeySequence::SaveAs); file_menu->addAction(tr("Copy DBC To Clipboard"), this, &MainWindow::saveDBCToClipboard); file_menu->addSeparator(); - file_menu->addAction(tr("Settings..."), this, &MainWindow::setOption); + file_menu->addAction(tr("Settings..."), this, &MainWindow::setOption)->setShortcuts(QKeySequence::Preferences); + + file_menu->addSeparator(); + file_menu->addAction(tr("E&xit"), qApp, &QApplication::closeAllWindows)->setShortcuts(QKeySequence::Quit); QMenu *edit_menu = menuBar()->addMenu(tr("&Edit")); - auto undo_act = detail_widget->undo_stack->createUndoAction(this, tr("&Undo")); + auto undo_act = UndoStack::instance()->createUndoAction(this, tr("&Undo")); undo_act->setShortcuts(QKeySequence::Undo); edit_menu->addAction(undo_act); - auto redo_act = detail_widget->undo_stack->createRedoAction(this, tr("&Rndo")); + auto redo_act = UndoStack::instance()->createRedoAction(this, tr("&Rndo")); redo_act->setShortcuts(QKeySequence::Redo); edit_menu->addAction(redo_act); edit_menu->addSeparator(); QMenu *commands_menu = edit_menu->addMenu(tr("Command &List")); - auto undo_view = new QUndoView(detail_widget->undo_stack); + auto undo_view = new QUndoView(UndoStack::instance()); undo_view->setWindowTitle(tr("Command List")); QWidgetAction *commands_act = new QWidgetAction(this); commands_act->setDefaultWidget(undo_view); @@ -101,54 +130,43 @@ void MainWindow::createActions() { void MainWindow::createDockWindows() { // left panel - QWidget *messages_container = new QWidget(this); - QVBoxLayout *messages_layout = new QVBoxLayout(messages_container); - dbc_combo = createDBCSelector(); - messages_layout->addWidget(dbc_combo); messages_widget = new MessagesWidget(this); - messages_layout->addWidget(messages_widget); - QDockWidget *dock = new QDockWidget(tr("MESSAGES"), this); dock->setObjectName("MessagesPanel"); dock->setAllowedAreas(Qt::LeftDockWidgetArea | Qt::RightDockWidgetArea | Qt::TopDockWidgetArea | Qt::BottomDockWidgetArea); dock->setFeatures(QDockWidget::DockWidgetMovable | QDockWidget::DockWidgetFloatable); - dock->setWidget(messages_container); + dock->setWidget(messages_widget); addDockWidget(Qt::LeftDockWidgetArea, dock); // right panel - QWidget *right_container = new QWidget(this); - r_layout = new QVBoxLayout(right_container); charts_widget = new ChartsWidget(this); + QWidget *charts_container = new QWidget(this); + charts_layout = new QVBoxLayout(charts_container); + charts_layout->setContentsMargins(0, 0, 0, 0); + charts_layout->addWidget(charts_widget); + + // splitter between video and charts + video_splitter = new QSplitter(Qt::Vertical, this); video_widget = new VideoWidget(this); - r_layout->addWidget(video_widget, 0, Qt::AlignTop); - r_layout->addWidget(charts_widget, 1); - r_layout->addStretch(0); + video_splitter->addWidget(video_widget); + QObject::connect(charts_widget, &ChartsWidget::rangeChanged, video_widget, &VideoWidget::rangeChanged); + + video_splitter->addWidget(charts_container); + video_splitter->setStretchFactor(1, 1); + video_splitter->restoreState(settings.video_splitter_state); + if (can->liveStreaming() || video_splitter->sizes()[0] == 0) { + // display video at minimum size. + video_splitter->setSizes({1, 1}); + } video_dock = new QDockWidget(can->routeName(), this); video_dock->setObjectName(tr("VideoPanel")); video_dock->setAllowedAreas(Qt::LeftDockWidgetArea | Qt::RightDockWidgetArea); video_dock->setFeatures(QDockWidget::DockWidgetMovable | QDockWidget::DockWidgetFloatable); - video_dock->setWidget(right_container); + video_dock->setWidget(video_splitter); addDockWidget(Qt::RightDockWidgetArea, video_dock); } -QComboBox *MainWindow::createDBCSelector() { - QComboBox *c = new QComboBox(this); - c->setEditable(true); - c->lineEdit()->setPlaceholderText(tr("Select from an existing DBC file")); - c->setInsertPolicy(QComboBox::NoInsert); - c->completer()->setCompletionMode(QCompleter::PopupCompletion); - c->completer()->setFilterMode(Qt::MatchContains); - - auto dbc_names = dbc()->allDBCNames(); - std::sort(dbc_names.begin(), dbc_names.end()); - for (const auto &name : dbc_names) { - c->addItem(QString::fromStdString(name)); - } - c->setCurrentIndex(-1); - return c; -} - void MainWindow::createStatusBar() { progress_bar = new QProgressBar(); progress_bar->setRange(0, 100); @@ -164,60 +182,140 @@ void MainWindow::createShortcuts() { // TODO: add more shortcuts here. } +void MainWindow::undoStackCleanChanged(bool clean) { + setWindowModified(!clean); +} + void MainWindow::DBCFileChanged() { - detail_widget->undo_stack->clear(); - int index = dbc_combo->findText(QFileInfo(dbc()->name()).baseName()); - dbc_combo->setCurrentIndex(index); - setWindowTitle(tr("%1 - Cabana").arg(dbc()->name())); + UndoStack::instance()->clear(); + setWindowFilePath(QString("%1").arg(dbc()->name())); } -void MainWindow::loadDBCFromName(const QString &name) { - if (name != dbc()->name()) { - dbc()->open(name); +void MainWindow::openRoute() { + OpenRouteDialog dlg(this); + if (dlg.exec()) { + detail_widget->removeAll(); + charts_widget->removeAll(); + statusBar()->showMessage(tr("Route %1 loaded").arg(can->routeName()), 2000); + } else if (dlg.failedToLoad()) { + close(); + } +} + +void MainWindow::newFile() { + remindSaveChanges(); + dbc()->open("untitled.dbc", ""); +} + +void MainWindow::openFile() { + remindSaveChanges(); + QString fn = QFileDialog::getOpenFileName(this, tr("Open File"), settings.last_dir, "DBC (*.dbc)"); + if (!fn.isEmpty()) { + loadFile(fn); } } -void MainWindow::loadDBCFromFile() { - QString file_name = QFileDialog::getOpenFileName(this, tr("Open File"), settings.last_dir, "DBC (*.dbc)"); - if (!file_name.isEmpty()) { - settings.last_dir = QFileInfo(file_name).absolutePath(); - QFile file(file_name); +void MainWindow::loadFile(const QString &fn) { + if (!fn.isEmpty()) { + QFile file(fn); if (file.open(QIODevice::ReadOnly)) { - auto dbc_name = QFileInfo(file_name).baseName(); - dbc()->open(dbc_name, file.readAll()); + auto dbc_name = QFileInfo(fn).baseName(); + QString error; + bool ret = dbc()->open(dbc_name, file.readAll(), &error); + if (ret) { + setCurrentFile(fn); + statusBar()->showMessage(tr("DBC File %1 loaded").arg(fn), 2000); + } else { + QMessageBox msg_box(QMessageBox::Warning, tr("Failed to load DBC file"), tr("Failed to parse DBC file %1").arg(fn)); + msg_box.setDetailedText(error); + msg_box.exec(); + } } } } +void MainWindow::openOpendbcFile() { + if (auto action = qobject_cast(sender())) { + remindSaveChanges(); + loadDBCFromOpendbc(action->text()); + } +} + +void MainWindow::openRecentFile() { + if (auto action = qobject_cast(sender())) { + remindSaveChanges(); + loadFile(action->data().toString()); + } +} + +void MainWindow::loadDBCFromOpendbc(const QString &name) { + if (name != dbc()->name()) { + remindSaveChanges(); + dbc()->open(name); + } +} + void MainWindow::loadDBCFromClipboard() { + remindSaveChanges(); QString dbc_str = QGuiApplication::clipboard()->text(); - dbc()->open("From Clipboard", dbc_str); - QMessageBox::information(this, tr("Load From Clipboard"), tr("DBC Successfully Loaded!")); + QString error; + bool ret = dbc()->open("clipboard", dbc_str, &error); + if (ret && dbc()->messages().size() > 0) { + QMessageBox::information(this, tr("Load From Clipboard"), tr("DBC Successfully Loaded!")); + } else { + QMessageBox msg_box(QMessageBox::Warning, tr("Failed to load DBC from clipboard"), tr("Make sure that you paste the text with correct format.")); + if (!error.isEmpty()) { + msg_box.setDetailedText(error); + } + msg_box.exec(); + } } void MainWindow::loadDBCFromFingerprint() { + // Don't overwrite already loaded DBC + if (!dbc()->name().isEmpty()) { + return; + } + + remindSaveChanges(); auto fingerprint = can->carFingerprint(); - video_dock->setWindowTitle(tr("ROUTE: %1 FINGERPINT: %2").arg(can->routeName()).arg(fingerprint.isEmpty() ? tr("Unknown Car") : fingerprint)); + if (can->liveStreaming()) { + video_dock->setWindowTitle(can->routeName()); + } else { + video_dock->setWindowTitle(tr("ROUTE: %1 FINGERPINT: %2").arg(can->routeName()).arg(fingerprint.isEmpty() ? tr("Unknown Car") : fingerprint)); + } if (!fingerprint.isEmpty()) { auto dbc_name = fingerprint_to_dbc[fingerprint]; if (dbc_name != QJsonValue::Undefined) { - loadDBCFromName(dbc_name.toString()); + loadDBCFromOpendbc(dbc_name.toString()); return; } } - dbc()->open("New_DBC", ""); + newFile(); } -void MainWindow::saveDBCToFile() { - QString file_name = QFileDialog::getSaveFileName(this, tr("Save File"), - QDir::cleanPath(settings.last_dir + "/untitled.dbc"), tr("DBC (*.dbc)")); - if (!file_name.isEmpty()) { - settings.last_dir = QFileInfo(file_name).absolutePath(); - QFile file(file_name); - if (file.open(QIODevice::WriteOnly)) { - file.write(dbc()->generateDBC().toUtf8()); - detail_widget->undo_stack->clear(); - } +void MainWindow::save() { + if (current_file.isEmpty()) { + saveAs(); + } else { + saveFile(current_file); + } +} + +void MainWindow::saveFile(const QString &fn) { + QFile file(fn); + if (file.open(QIODevice::WriteOnly)) { + file.write(dbc()->generateDBC().toUtf8()); + UndoStack::instance()->setClean(); + setCurrentFile(fn); + statusBar()->showMessage(tr("File saved"), 2000); + } +} + +void MainWindow::saveAs() { + QString fn = QFileDialog::getSaveFileName(this, tr("Save File"), QDir::cleanPath(settings.last_dir + "/untitled.dbc"), tr("DBC (*.dbc)")); + if (!fn.isEmpty()) { + saveFile(fn); } } @@ -226,6 +324,49 @@ void MainWindow::saveDBCToClipboard() { QMessageBox::information(this, tr("Copy To Clipboard"), tr("DBC Successfully copied!")); } +void MainWindow::setCurrentFile(const QString &fn) { + current_file = fn; + setWindowFilePath(QString("%1").arg(fn)); + settings.recent_files.removeAll(fn); + settings.recent_files.prepend(fn); + while (settings.recent_files.size() > MAX_RECENT_FILES) { + settings.recent_files.removeLast(); + } + settings.last_dir = QFileInfo(fn).absolutePath(); + updateRecentFileActions(); +} + +void MainWindow::updateRecentFileActions() { + int num_recent_files = std::min(settings.recent_files.size(), MAX_RECENT_FILES); + + for (int i = 0; i < num_recent_files; ++i) { + QString text = tr("&%1 %2").arg(i + 1).arg(QFileInfo(settings.recent_files[i]).fileName()); + recent_files_acts[i]->setText(text); + recent_files_acts[i]->setData(settings.recent_files[i]); + recent_files_acts[i]->setVisible(true); + } + for (int i = num_recent_files; i < MAX_RECENT_FILES; ++i) { + recent_files_acts[i]->setVisible(false); + } + open_recent_menu->setEnabled(num_recent_files > 0); +} + +void MainWindow::remindSaveChanges() { + bool discard_changes = false; + while (!UndoStack::instance()->isClean() && !discard_changes) { + int ret = (QMessageBox::question(this, tr("Unsaved Changes"), + tr("You have unsaved changes. Press ok to save them, cancel to discard."), + QMessageBox::Ok | QMessageBox::Cancel)); + if (ret == QMessageBox::Ok) { + save(); + } else { + discard_changes = true; + } + } + UndoStack::instance()->clear(); + current_file = ""; +} + void MainWindow::updateDownloadProgress(uint64_t cur, uint64_t total, bool success) { if (success && cur < total) { progress_bar->setValue((cur / (double)total) * 100); @@ -239,13 +380,13 @@ void MainWindow::updateDownloadProgress(uint64_t cur, uint64_t total, bool succe void MainWindow::dockCharts(bool dock) { if (dock && floating_window) { floating_window->removeEventFilter(charts_widget); - r_layout->insertWidget(2, charts_widget, 1); + charts_layout->insertWidget(0, charts_widget, 1); floating_window->deleteLater(); floating_window = nullptr; } else if (!dock && !floating_window) { floating_window = new QWidget(this); floating_window->setWindowFlags(Qt::Window); - floating_window->setWindowTitle("Charts - Cabana"); + floating_window->setWindowTitle("Charts"); floating_window->setLayout(new QVBoxLayout()); floating_window->layout()->addWidget(charts_widget); floating_window->installEventFilter(charts_widget); @@ -254,22 +395,19 @@ void MainWindow::dockCharts(bool dock) { } void MainWindow::closeEvent(QCloseEvent *event) { - if (detail_widget->undo_stack->index() > 0) { - auto ret = QMessageBox::question(this, tr("Unsaved Changes"), - tr("Are you sure you want to exit without saving?\nAny unsaved changes will be lost."), - QMessageBox::Yes | QMessageBox::No); - if (ret == QMessageBox::No) { - event->ignore(); - return; - } - } + remindSaveChanges(); main_win = nullptr; if (floating_window) floating_window->deleteLater(); + // save states settings.geometry = saveGeometry(); settings.window_state = saveState(); + if (!can->liveStreaming()) { + settings.video_splitter_state = video_splitter->saveState(); + } + settings.message_header_state = messages_widget->saveHeaderState(); settings.save(); QWidget::closeEvent(event); } diff --git a/tools/cabana/mainwin.h b/tools/cabana/mainwin.h index f8b5f92349..5e627df58b 100644 --- a/tools/cabana/mainwin.h +++ b/tools/cabana/mainwin.h @@ -1,10 +1,10 @@ #pragma once -#include #include #include #include #include +#include #include #include "tools/cabana/chartswidget.h" @@ -20,13 +20,19 @@ public: MainWindow(); void dockCharts(bool dock); void showStatusMessage(const QString &msg, int timeout = 0) { statusBar()->showMessage(msg, timeout); } + void loadFile(const QString &fn); public slots: - void loadDBCFromName(const QString &name); + void openRoute(); + void newFile(); + void openFile(); + void openRecentFile(); + void openOpendbcFile(); + void loadDBCFromOpendbc(const QString &name); void loadDBCFromFingerprint(); - void loadDBCFromFile(); void loadDBCFromClipboard(); - void saveDBCToFile(); + void save(); + void saveAs(); void saveDBCToClipboard(); signals: @@ -34,9 +40,12 @@ signals: void updateProgressBar(uint64_t cur, uint64_t total, bool success); protected: + void remindSaveChanges(); + void saveFile(const QString &fn); + void setCurrentFile(const QString &fn); + void updateRecentFileActions(); void createActions(); void createDockWindows(); - QComboBox *createDBCSelector(); void createStatusBar(); void createShortcuts(); void closeEvent(QCloseEvent *event) override; @@ -44,15 +53,20 @@ protected: void updateDownloadProgress(uint64_t cur, uint64_t total, bool success); void setOption(); void findSimilarBits(); + void undoStackCleanChanged(bool clean); - VideoWidget *video_widget; + VideoWidget *video_widget = nullptr; QDockWidget *video_dock; MessagesWidget *messages_widget; DetailWidget *detail_widget; ChartsWidget *charts_widget; QWidget *floating_window = nullptr; - QVBoxLayout *r_layout; + QVBoxLayout *charts_layout; QProgressBar *progress_bar; QJsonDocument fingerprint_to_dbc; - QComboBox *dbc_combo; + QSplitter *video_splitter;; + QString current_file = ""; + enum { MAX_RECENT_FILES = 15 }; + QAction *recent_files_acts[MAX_RECENT_FILES] = {}; + QMenu *open_recent_menu = nullptr; }; diff --git a/tools/cabana/messageswidget.cc b/tools/cabana/messageswidget.cc index fd0bc68514..8279d08a5c 100644 --- a/tools/cabana/messageswidget.cc +++ b/tools/cabana/messageswidget.cc @@ -1,18 +1,19 @@ #include "tools/cabana/messageswidget.h" +#include #include -#include -#include +#include +#include +#include #include #include "tools/cabana/dbcmanager.h" MessagesWidget::MessagesWidget(QWidget *parent) : QWidget(parent) { QVBoxLayout *main_layout = new QVBoxLayout(this); - main_layout->setContentsMargins(0, 0, 0, 0); // message filter - QLineEdit *filter = new QLineEdit(this); + filter = new QLineEdit(this); filter->setClearButtonEnabled(true); filter->setPlaceholderText(tr("filter messages")); main_layout->addWidget(filter); @@ -21,21 +22,27 @@ MessagesWidget::MessagesWidget(QWidget *parent) : QWidget(parent) { table_widget = new QTableView(this); model = new MessageListModel(this); table_widget->setModel(model); + table_widget->setItemDelegateForColumn(4, new MessageBytesDelegate(table_widget)); table_widget->setSelectionBehavior(QAbstractItemView::SelectRows); table_widget->setSelectionMode(QAbstractItemView::SingleSelection); - table_widget->setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Expanding); table_widget->setSortingEnabled(true); table_widget->sortByColumn(0, Qt::AscendingOrder); - table_widget->setColumnWidth(0, 250); - table_widget->setColumnWidth(1, 80); - table_widget->setColumnWidth(2, 80); table_widget->horizontalHeader()->setStretchLastSection(true); table_widget->verticalHeader()->hide(); main_layout->addWidget(table_widget); + // suppress + QHBoxLayout *suppress_layout = new QHBoxLayout(); + suppress_add = new QPushButton("Suppress Highlighted"); + suppress_clear = new QPushButton(); + suppress_layout->addWidget(suppress_add); + suppress_layout->addWidget(suppress_clear); + main_layout->addLayout(suppress_layout); + // signals/slots - QObject::connect(filter, &QLineEdit::textChanged, model, &MessageListModel::setFilterString); - QObject::connect(can, &CANMessages::msgsReceived, model, &MessageListModel::msgsReceived); + QObject::connect(filter, &QLineEdit::textEdited, model, &MessageListModel::setFilterString); + QObject::connect(can, &AbstractStream::msgsReceived, model, &MessageListModel::msgsReceived); + QObject::connect(can, &AbstractStream::streamStarted, this, &MessagesWidget::reset); QObject::connect(dbc(), &DBCManager::DBCFileChanged, model, &MessageListModel::sortMessages); QObject::connect(dbc(), &DBCManager::msgUpdated, model, &MessageListModel::sortMessages); QObject::connect(dbc(), &DBCManager::msgRemoved, model, &MessageListModel::sortMessages); @@ -48,6 +55,16 @@ MessagesWidget::MessagesWidget(QWidget *parent) : QWidget(parent) { } } }); + QObject::connect(suppress_add, &QPushButton::clicked, [=]() { + model->suppress(); + updateSuppressedButtons(); + }); + QObject::connect(suppress_clear, &QPushButton::clicked, [=]() { + model->clearSuppress(); + updateSuppressedButtons(); + }); + + updateSuppressedButtons(); } void MessagesWidget::selectMessage(const QString &msg_id) { @@ -56,6 +73,24 @@ void MessagesWidget::selectMessage(const QString &msg_id) { } } +void MessagesWidget::updateSuppressedButtons() { + if (model->suppressed_bytes.empty()) { + suppress_clear->setEnabled(false); + suppress_clear->setText("Clear Suppressed"); + } else { + suppress_clear->setEnabled(true); + suppress_clear->setText(QString("Clear Suppressed (%1)").arg(model->suppressed_bytes.size())); + } +} + +void MessagesWidget::reset() { + model->reset(); + filter->clear(); + current_msg_id = ""; + updateSuppressedButtons(); +} + + // MessageListModel QVariant MessageListModel::headerData(int section, Qt::Orientation orientation, int role) const { @@ -65,9 +100,10 @@ QVariant MessageListModel::headerData(int section, Qt::Orientation orientation, } QVariant MessageListModel::data(const QModelIndex &index, int role) const { + const auto &id = msgs[index.row()]; + auto &can_data = can->lastMessage(id); + if (role == Qt::DisplayRole) { - const auto &id = msgs[index.row()]; - auto &can_data = can->lastMessage(id); switch (index.column()) { case 0: return msgName(id); case 1: return id; @@ -75,17 +111,39 @@ QVariant MessageListModel::data(const QModelIndex &index, int role) const { case 3: return can_data.count; case 4: return toHex(can_data.dat); } - } else if (role == Qt::FontRole && index.column() == columnCount() - 1) { - return QFontDatabase::systemFont(QFontDatabase::FixedFont); + } else if (role == Qt::UserRole && index.column() == 4) { + QList colors; + colors.reserve(can_data.dat.size()); + for (int i = 0; i < can_data.dat.size(); i++){ + if (suppressed_bytes.contains({id, i})) { + colors.append(QColor(255, 255, 255, 0)); + } else { + colors.append(i < can_data.colors.size() ? can_data.colors[i] : QColor(255, 255, 255, 0)); + } + } + return colors; + } return {}; } void MessageListModel::setFilterString(const QString &string) { + auto contains = [](const QString &id, const QString &txt) { + auto cs = Qt::CaseInsensitive; + if (id.contains(txt, cs) || msgName(id).contains(txt, cs)) return true; + // Search by signal name + if (const auto msg = dbc()->msg(id)) { + for (auto &signal : msg->getSignals()) { + if (QString::fromStdString(signal->name).contains(txt, cs)) return true; + } + } + return false; + }; + filter_str = string; msgs.clear(); for (auto it = can->can_msgs.begin(); it != can->can_msgs.end(); ++it) { - if (it.key().contains(filter_str, Qt::CaseInsensitive) || msgName(it.key()).contains(filter_str, Qt::CaseInsensitive)) { + if (filter_str.isEmpty() || contains(it.key(), filter_str)) { msgs.push_back(it.key()); } } @@ -96,22 +154,27 @@ void MessageListModel::sortMessages() { beginResetModel(); if (sort_column == 0) { std::sort(msgs.begin(), msgs.end(), [this](auto &l, auto &r) { - bool ret = std::pair{msgName(l), l} < std::pair{msgName(r), r}; - return sort_order == Qt::AscendingOrder ? ret : !ret; + auto ll = std::pair{msgName(l), l}; + auto rr = std::pair{msgName(r), r}; + return sort_order == Qt::AscendingOrder ? ll < rr : ll > rr; }); } else if (sort_column == 1) { std::sort(msgs.begin(), msgs.end(), [this](auto &l, auto &r) { - return sort_order == Qt::AscendingOrder ? l < r : l > r; + auto ll = DBCManager::parseId(l); + auto rr = DBCManager::parseId(r); + return sort_order == Qt::AscendingOrder ? ll < rr : ll > rr; }); } else if (sort_column == 2) { std::sort(msgs.begin(), msgs.end(), [this](auto &l, auto &r) { - bool ret = std::pair{can->lastMessage(l).freq, l} < std::pair{can->lastMessage(r).freq, r}; - return sort_order == Qt::AscendingOrder ? ret : !ret; + auto ll = std::pair{can->lastMessage(l).freq, l}; + auto rr = std::pair{can->lastMessage(r).freq, r}; + return sort_order == Qt::AscendingOrder ? ll < rr : ll > rr; }); } else if (sort_column == 3) { std::sort(msgs.begin(), msgs.end(), [this](auto &l, auto &r) { - bool ret = std::pair{can->lastMessage(l).count, l} < std::pair{can->lastMessage(r).count, r}; - return sort_order == Qt::AscendingOrder ? ret : !ret; + auto ll = std::pair{can->lastMessage(l).count, l}; + auto rr = std::pair{can->lastMessage(r).count, r}; + return sort_order == Qt::AscendingOrder ? ll < rr : ll > rr; }); } endResetModel(); @@ -141,3 +204,29 @@ void MessageListModel::sort(int column, Qt::SortOrder order) { sortMessages(); } } + +void MessageListModel::suppress() { + const double cur_ts = can->currentSec(); + + for (auto &id : msgs) { + auto &can_data = can->lastMessage(id); + for (int i = 0; i < can_data.dat.size(); i++) { + const double dt = cur_ts - can_data.last_change_t[i]; + if (dt < 2.0) { + suppressed_bytes.insert({id, i}); + } + } + } +} + +void MessageListModel::clearSuppress() { + suppressed_bytes.clear(); +} + +void MessageListModel::reset() { + beginResetModel(); + filter_str = ""; + msgs.clear(); + clearSuppress(); + endResetModel(); +} diff --git a/tools/cabana/messageswidget.h b/tools/cabana/messageswidget.h index 62aa23c02e..81ee36cd6f 100644 --- a/tools/cabana/messageswidget.h +++ b/tools/cabana/messageswidget.h @@ -1,9 +1,13 @@ #pragma once #include +#include +#include +#include +#include #include -#include "tools/cabana/canmessages.h" +#include "tools/cabana/streams/abstractstream.h" class MessageListModel : public QAbstractTableModel { Q_OBJECT @@ -18,7 +22,11 @@ public: void setFilterString(const QString &string); void msgsReceived(const QHash *new_msgs = nullptr); void sortMessages(); + void suppress(); + void clearSuppress(); + void reset(); QStringList msgs; + QSet> suppressed_bytes; private: QString filter_str; @@ -32,6 +40,10 @@ class MessagesWidget : public QWidget { public: MessagesWidget(QWidget *parent); void selectMessage(const QString &message_id); + QByteArray saveHeaderState() const { return table_widget->horizontalHeader()->saveState(); } + bool restoreHeaderState(const QByteArray &state) const { return table_widget->horizontalHeader()->restoreState(state); } + void updateSuppressedButtons(); + void reset(); signals: void msgSelectionChanged(const QString &message_id); @@ -39,5 +51,9 @@ signals: protected: QTableView *table_widget; QString current_msg_id; + QLineEdit *filter; MessageListModel *model; + QPushButton *suppress_add; + QPushButton *suppress_clear; + }; diff --git a/tools/cabana/route.cc b/tools/cabana/route.cc new file mode 100644 index 0000000000..ab322cdf90 --- /dev/null +++ b/tools/cabana/route.cc @@ -0,0 +1,68 @@ +#include "tools/cabana/route.h" + +#include +#include +#include +#include +#include +#include + +#include "tools/cabana/streams/replaystream.h" + +OpenRouteDialog::OpenRouteDialog(QWidget *parent) : QDialog(parent) { + // TODO: get route list from api.comma.ai + QHBoxLayout *edit_layout = new QHBoxLayout; + edit_layout->addWidget(new QLabel(tr("Route:"))); + edit_layout->addWidget(route_edit = new QLineEdit(this)); + route_edit->setPlaceholderText(tr("Enter remote route name or click browse to select a local route")); + auto file_btn = new QPushButton(tr("Browse..."), this); + edit_layout->addWidget(file_btn); + + btn_box = new QDialogButtonBox(QDialogButtonBox::Open | QDialogButtonBox::Cancel); + btn_box->button(QDialogButtonBox::Open)->setEnabled(false); + + QVBoxLayout *main_layout = new QVBoxLayout(this); + main_layout->addStretch(0); + main_layout->addLayout(edit_layout); + main_layout->addStretch(0); + main_layout->addWidget(btn_box); + setMinimumSize({550, 120}); + + QObject::connect(btn_box, &QDialogButtonBox::accepted, this, &OpenRouteDialog::loadRoute); + QObject::connect(btn_box, &QDialogButtonBox::rejected, this, &QDialog::reject); + QObject::connect(route_edit, &QLineEdit::textChanged, [this]() { + btn_box->button(QDialogButtonBox::Open)->setEnabled(!route_edit->text().isEmpty()); + }); + QObject::connect(file_btn, &QPushButton::clicked, [=]() { + QString dir = QFileDialog::getExistingDirectory(this, tr("Open Local Route"), settings.last_route_dir); + if (!dir.isEmpty()) { + route_edit->setText(dir); + settings.last_route_dir = QFileInfo(dir).absolutePath(); + } + }); +} + +void OpenRouteDialog::loadRoute() { + btn_box->setEnabled(false); + + QString route = route_edit->text(); + QString data_dir; + if (int idx = route.lastIndexOf('/'); idx != -1) { + data_dir = route.mid(0, idx + 1); + route = route.mid(idx + 1); + } + + bool is_valid_format = Route::parseRoute(route).str.size() > 0; + if (!is_valid_format) { + QMessageBox::warning(nullptr, tr("Warning"), tr("Invalid route format: '%1'").arg(route)); + } else { + failed_to_load = !dynamic_cast(can)->loadRoute(route, data_dir); + if (failed_to_load) { + QMessageBox::warning(nullptr, tr("Warning"), tr("Failed to load route: '%1'").arg(route)); + } else { + accept(); + } + } + + btn_box->setEnabled(true); +} diff --git a/tools/cabana/route.h b/tools/cabana/route.h new file mode 100644 index 0000000000..ceda71d585 --- /dev/null +++ b/tools/cabana/route.h @@ -0,0 +1,19 @@ +#pragma once + +#include +#include +#include + +class OpenRouteDialog : public QDialog { + Q_OBJECT + +public: + OpenRouteDialog(QWidget *parent); + void loadRoute(); + inline bool failedToLoad() const { return failed_to_load; } + +private: + QLineEdit *route_edit; + QDialogButtonBox *btn_box; + bool failed_to_load = false; +}; diff --git a/tools/cabana/settings.cc b/tools/cabana/settings.cc index 5e7f833317..6cbd16cabf 100644 --- a/tools/cabana/settings.cc +++ b/tools/cabana/settings.cc @@ -15,23 +15,35 @@ Settings::Settings() { void Settings::save() { QSettings s("settings", QSettings::IniFormat); s.setValue("fps", fps); - s.setValue("cached_segment", cached_segment_limit); + s.setValue("max_cached_minutes", max_cached_minutes); s.setValue("chart_height", chart_height); - s.setValue("max_chart_x_range", max_chart_x_range); + s.setValue("chart_range", chart_range); + s.setValue("chart_column_count", chart_column_count); s.setValue("last_dir", last_dir); + s.setValue("last_route_dir", last_route_dir); s.setValue("window_state", window_state); s.setValue("geometry", geometry); + s.setValue("video_splitter_state", video_splitter_state); + s.setValue("recent_files", recent_files); + s.setValue("message_header_state", message_header_state); + s.setValue("chart_series_type", chart_series_type); } void Settings::load() { QSettings s("settings", QSettings::IniFormat); fps = s.value("fps", 10).toInt(); - cached_segment_limit = s.value("cached_segment", 3).toInt(); + max_cached_minutes = s.value("max_cached_minutes", 5).toInt(); chart_height = s.value("chart_height", 200).toInt(); - max_chart_x_range = s.value("max_chart_x_range", 3 * 60).toInt(); + chart_range = s.value("chart_range", 3 * 60).toInt(); + chart_column_count = s.value("chart_column_count", 1).toInt(); last_dir = s.value("last_dir", QDir::homePath()).toString(); + last_route_dir = s.value("last_route_dir", QDir::homePath()).toString(); window_state = s.value("window_state").toByteArray(); geometry = s.value("geometry").toByteArray(); + video_splitter_state = s.value("video_splitter_state").toByteArray(); + recent_files = s.value("recent_files").toStringList(); + message_header_state = s.value("message_header_state").toByteArray(); + chart_series_type = s.value("chart_series_type", 0).toInt(); } // SettingsDlg @@ -46,23 +58,22 @@ SettingsDlg::SettingsDlg(QWidget *parent) : QDialog(parent) { fps->setValue(settings.fps); form_layout->addRow("FPS", fps); - cached_segment = new QSpinBox(this); - cached_segment->setRange(3, 60); - cached_segment->setSingleStep(1); - cached_segment->setValue(settings.cached_segment_limit); - form_layout->addRow(tr("Cached segments limit"), cached_segment); + cached_minutes = new QSpinBox(this); + cached_minutes->setRange(5, 60); + cached_minutes->setSingleStep(1); + cached_minutes->setValue(settings.max_cached_minutes); + form_layout->addRow(tr("Max Cached Minutes"), cached_minutes); - max_chart_x_range = new QSpinBox(this); - max_chart_x_range->setRange(1, 60); - max_chart_x_range->setSingleStep(1); - max_chart_x_range->setValue(settings.max_chart_x_range / 60); - form_layout->addRow(tr("Chart range (minutes)"), max_chart_x_range); + chart_series_type = new QComboBox(this); + chart_series_type->addItems({tr("Line"), tr("Scatter")}); + chart_series_type->setCurrentIndex(settings.chart_series_type); + form_layout->addRow(tr("Chart Default Series Type"), chart_series_type); chart_height = new QSpinBox(this); chart_height->setRange(100, 500); chart_height->setSingleStep(10); chart_height->setValue(settings.chart_height); - form_layout->addRow(tr("Chart height"), chart_height); + form_layout->addRow(tr("Chart Height"), chart_height); auto buttonBox = new QDialogButtonBox(QDialogButtonBox::Ok | QDialogButtonBox::Cancel); form_layout->addRow(buttonBox); @@ -74,9 +85,9 @@ SettingsDlg::SettingsDlg(QWidget *parent) : QDialog(parent) { void SettingsDlg::save() { settings.fps = fps->value(); - settings.cached_segment_limit = cached_segment->value(); + settings.max_cached_minutes = cached_minutes->value(); + settings.chart_series_type = chart_series_type->currentIndex(); settings.chart_height = chart_height->value(); - settings.max_chart_x_range = max_chart_x_range->value() * 60; settings.save(); accept(); emit settings.changed(); diff --git a/tools/cabana/settings.h b/tools/cabana/settings.h index d231a3a53a..a302d20077 100644 --- a/tools/cabana/settings.h +++ b/tools/cabana/settings.h @@ -14,11 +14,18 @@ public: void load(); int fps = 10; - int cached_segment_limit = 3; + int max_cached_minutes = 5; int chart_height = 200; - int max_chart_x_range = 3 * 60; // 3 minutes + int chart_column_count = 1; + int chart_range = 3 * 60; // e minutes + int chart_series_type = 0; QString last_dir; - QByteArray window_state, geometry; + QString last_route_dir; + QByteArray geometry; + QByteArray video_splitter_state; + QByteArray window_state; + QStringList recent_files; + QByteArray message_header_state; signals: void changed(); @@ -31,9 +38,9 @@ public: SettingsDlg(QWidget *parent); void save(); QSpinBox *fps; - QSpinBox *cached_segment; + QSpinBox *cached_minutes; QSpinBox *chart_height; - QSpinBox *max_chart_x_range; + QComboBox *chart_series_type; }; extern Settings settings; diff --git a/tools/cabana/signaledit.cc b/tools/cabana/signaledit.cc index 3845e72be1..0499b1be8a 100644 --- a/tools/cabana/signaledit.cc +++ b/tools/cabana/signaledit.cc @@ -1,158 +1,192 @@ #include "tools/cabana/signaledit.h" -#include -#include #include #include +#include +#include +#include +#include #include -// SignalForm +#include "tools/cabana/commands.h" -SignalForm::SignalForm(QWidget *parent) : QWidget(parent) { - QFormLayout *form_layout = new QFormLayout(this); +// SignalModel - name = new QLineEdit(); - name->setValidator(new QRegExpValidator(QRegExp("^(\\w+)"), name)); - form_layout->addRow(tr("Name"), name); +SignalModel::SignalModel(QObject *parent) : root(new Item), QAbstractItemModel(parent) { + QObject::connect(dbc(), &DBCManager::DBCFileChanged, this, &SignalModel::refresh); + QObject::connect(dbc(), &DBCManager::msgUpdated, this, &SignalModel::handleMsgChanged); + QObject::connect(dbc(), &DBCManager::msgRemoved, this, &SignalModel::handleMsgChanged); + QObject::connect(dbc(), &DBCManager::signalAdded, this, &SignalModel::handleSignalAdded); + QObject::connect(dbc(), &DBCManager::signalUpdated, this, &SignalModel::handleSignalUpdated); + QObject::connect(dbc(), &DBCManager::signalRemoved, this, &SignalModel::handleSignalRemoved); + QObject::connect(can, &AbstractStream::msgsReceived, this, &SignalModel::updateState); +} - size = new QSpinBox(); - size->setMinimum(1); - form_layout->addRow(tr("Size"), size); +void SignalModel::insertItem(SignalModel::Item *parent_item, int pos, const Signal *sig) { + Item *item = new Item{.sig = sig, .parent = parent_item, .title = sig->name.c_str(), .type = Item::Sig}; + parent_item->children.insert(pos, item); + QString titles[]{"Name", "Size", "Little Endian", "Signed", "Offset", "Factor", "Extra Info", "Unit", "Comment", "Minimum", "Maximum", "Description"}; + for (int i = 0; i < std::size(titles); ++i) { + item->children.push_back(new Item{.sig = sig, .parent = item, .title = titles[i], .type = (Item::Type)(i + Item::Name)}); + } +} - endianness = new QComboBox(); - endianness->addItems({"Little", "Big"}); - form_layout->addRow(tr("Endianness"), endianness); +void SignalModel::setMessage(const QString &id) { + msg_id = id; + filter_str = ""; + refresh(); + updateState(nullptr); +} - form_layout->addRow(tr("lsb"), lsb = new QLabel()); - form_layout->addRow(tr("msb"), msb = new QLabel()); +void SignalModel::setFilter(const QString &txt) { + filter_str = txt; + refresh(); +} - sign = new QComboBox(); - sign->addItems({"Signed", "Unsigned"}); - form_layout->addRow(tr("sign"), sign); +void SignalModel::refresh() { + beginResetModel(); + root.reset(new SignalModel::Item); + if (auto msg = dbc()->msg(msg_id)) { + for (auto &s : msg->getSignals()) { + if (filter_str.isEmpty() || QString::fromStdString(s->name).contains(filter_str, Qt::CaseInsensitive)) { + insertItem(root.get(), root->children.size(), s); + } + } + } + endResetModel(); +} - auto double_validator = new QDoubleValidator(this); +void SignalModel::updateState(const QHash *msgs) { + if (!msgs || (msgs->contains(msg_id))) { + auto &dat = can->lastMessage(msg_id).dat; + int row = 0; + for (auto item : root->children) { + double value = get_raw_value((uint8_t *)dat.begin(), dat.size(), *item->sig); + item->sig_val = QString::number(value); + emit dataChanged(index(row, 1), index(row, 1), {Qt::DisplayRole}); + ++row; + } + } +} - factor = new QLineEdit(); - factor->setValidator(double_validator); - form_layout->addRow(tr("Factor"), factor); +int SignalModel::rowCount(const QModelIndex &parent) const { + if (parent.column() > 0) return 0; - offset = new QLineEdit(); - offset->setValidator(double_validator); - form_layout->addRow(tr("Offset"), offset); + auto parent_item = getItem(parent); + int row_count = parent_item->children.size(); + if (parent_item->type == Item::Sig && !parent_item->extra_expanded) { + row_count -= (Item::Desc - Item::ExtraInfo); + } + return row_count; +} - // TODO: parse the following parameters in opendbc - unit = new QLineEdit(); - form_layout->addRow(tr("Unit"), unit); - comment = new QLineEdit(); - form_layout->addRow(tr("Comment"), comment); - min_val = new QLineEdit(); - min_val->setValidator(double_validator); - form_layout->addRow(tr("Minimum value"), min_val); - max_val = new QLineEdit(); - max_val->setValidator(double_validator); - form_layout->addRow(tr("Maximum value"), max_val); - val_desc = new QLineEdit(); - form_layout->addRow(tr("Value descriptions"), val_desc); +Qt::ItemFlags SignalModel::flags(const QModelIndex &index) const { + if (!index.isValid()) return Qt::NoItemFlags; - QObject::connect(name, &QLineEdit::editingFinished, this, &SignalForm::textBoxEditingFinished); - QObject::connect(factor, &QLineEdit::editingFinished, this, &SignalForm::textBoxEditingFinished); - QObject::connect(offset, &QLineEdit::editingFinished, this, &SignalForm::textBoxEditingFinished); - QObject::connect(size, &QSpinBox::editingFinished, this, &SignalForm::changed); - QObject::connect(sign, SIGNAL(activated(int)), SIGNAL(changed())); - QObject::connect(endianness, SIGNAL(activated(int)), SIGNAL(changed())); + auto item = getItem(index); + Qt::ItemFlags flags = Qt::ItemIsSelectable | Qt::ItemIsEnabled; + if (index.column() == 1 && item->type != Item::Sig && item->type != Item::ExtraInfo) { + flags |= (item->type == Item::Endian || item->type == Item::Signed) ? Qt::ItemIsUserCheckable : Qt::ItemIsEditable; + } + return flags; } -void SignalForm::textBoxEditingFinished() { - QLineEdit *edit = qobject_cast(QObject::sender()); - if (edit && edit->isModified()) { - edit->setModified(false); - emit changed(); +int SignalModel::signalRow(const Signal *sig) const { + auto &children = root->children; + for (int i = 0; i < children.size(); ++i) { + if (children[i]->sig == sig) return i; } + return -1; } -// SignalEdit +QModelIndex SignalModel::index(int row, int column, const QModelIndex &parent) const { + if (!hasIndex(row, column, parent)) return {}; + return createIndex(row, column, getItem(parent)->children[row]); +} -SignalEdit::SignalEdit(int index, QWidget *parent) : form_idx(index), QWidget(parent) { - QVBoxLayout *main_layout = new QVBoxLayout(this); - main_layout->setContentsMargins(0, 0, 0, 0); - main_layout->setSpacing(0); +QModelIndex SignalModel::parent(const QModelIndex &index) const { + if (!index.isValid()) return {}; + Item *parent_item = getItem(index)->parent; + return parent_item == root.get() ? QModelIndex() : createIndex(parent_item->row(), 0, parent_item); +} - // title bar - auto title_bar = new QWidget(this); - title_bar->setFixedHeight(32); - QHBoxLayout *title_layout = new QHBoxLayout(title_bar); - title_layout->setContentsMargins(0, 0, 0, 0); - title_bar->setStyleSheet("QToolButton {width:15px;height:15px;font-size:15px}"); - color_label = new QLabel(this); - color_label->setFixedWidth(25); - color_label->setContentsMargins(5, 0, 0, 0); - title_layout->addWidget(color_label); - icon = new QLabel(this); - title_layout->addWidget(icon); - title = new ElidedLabel(this); - title->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Preferred); - title_layout->addWidget(title); - - plot_btn = new QToolButton(this); - plot_btn->setText("📈"); - plot_btn->setCheckable(true); - plot_btn->setAutoRaise(true); - title_layout->addWidget(plot_btn); - auto remove_btn = new QToolButton(this); - remove_btn->setAutoRaise(true); - remove_btn->setText("x"); - remove_btn->setToolTip(tr("Remove signal")); - title_layout->addWidget(remove_btn); - main_layout->addWidget(title_bar); +QVariant SignalModel::data(const QModelIndex &index, int role) const { + if (index.isValid()) { + const Item *item = getItem(index); + if (role == Qt::DisplayRole || role == Qt::EditRole) { + if (index.column() == 0) { + return item->type == Item::Sig ? QString::fromStdString(item->sig->name) : item->title; + } else { + switch (item->type) { + case Item::Sig: return item->sig_val; + case Item::Name: return QString::fromStdString(item->sig->name); + case Item::Size: return item->sig->size; + case Item::Offset: return QString::number(item->sig->offset, 'f', 6); + case Item::Factor: return QString::number(item->sig->factor, 'f', 6); + default: break; + } + } + } else if (role == Qt::CheckStateRole && index.column() == 1) { + if (item->type == Item::Endian) return item->sig->is_little_endian ? Qt::Checked : Qt::Unchecked; + if (item->type == Item::Signed) return item->sig->is_signed ? Qt::Checked : Qt::Unchecked; + } else if (role == Qt::DecorationRole && index.column() == 0 && item->type == Item::ExtraInfo) { + return utils::icon(item->parent->extra_expanded ? "chevron-compact-down" : "chevron-compact-up"); + } + } + return {}; +} - // signal form - form = new SignalForm(this); - form->setVisible(false); - main_layout->addWidget(form); - - // bottom line - QFrame *hline = new QFrame(); - hline->setFrameShape(QFrame::HLine); - hline->setFrameShadow(QFrame::Sunken); - main_layout->addWidget(hline); - - QObject::connect(title, &ElidedLabel::clicked, [this]() { emit showFormClicked(sig); }); - QObject::connect(plot_btn, &QToolButton::clicked, [this](bool checked) { - emit showChart(msg_id, sig, checked, QGuiApplication::keyboardModifiers() & Qt::ShiftModifier); - }); - QObject::connect(remove_btn, &QToolButton::clicked, [this]() { emit remove(sig); }); - QObject::connect(form, &SignalForm::changed, this, &SignalEdit::saveSignal); - setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Fixed); -} - -void SignalEdit::setSignal(const QString &message_id, const Signal *signal) { - sig = signal; - updateForm(msg_id == message_id && form->isVisible()); - msg_id = message_id; - color_label->setText(QString::number(form_idx + 1)); - color_label->setStyleSheet(QString("color:black; background-color:%2").arg(getColor(form_idx))); - title->setText(sig->name.c_str()); - show(); -} - -void SignalEdit::saveSignal() { - Signal s = *sig; - s.name = form->name->text().toStdString(); - s.size = form->size->text().toInt(); - s.offset = form->offset->text().toDouble(); - s.factor = form->factor->text().toDouble(); - s.is_signed = form->sign->currentIndex() == 0; - bool little_endian = form->endianness->currentIndex() == 0; - if (little_endian != s.is_little_endian) { +bool SignalModel::setData(const QModelIndex &index, const QVariant &value, int role) { + if (role != Qt::EditRole && role != Qt::CheckStateRole) return false; + + Item *item = getItem(index); + Signal s = *item->sig; + switch (item->type) { + case Item::Name: s.name = value.toString().toStdString(); break; + case Item::Size: s.size = value.toInt(); break; + case Item::Endian: s.is_little_endian = value.toBool(); break; + case Item::Signed: s.is_signed = value.toBool(); break; + case Item::Offset: s.offset = value.toDouble(); break; + case Item::Factor: s.factor = value.toDouble(); break; + default: return false; + } + bool ret = saveSignal(item->sig, s); + emit dataChanged(index, index, {Qt::DisplayRole, Qt::EditRole, Qt::CheckStateRole}); + return ret; +} + +void SignalModel::showExtraInfo(const QModelIndex &index) { + auto item = getItem(index); + if (item->type == Item::ExtraInfo) { + if (!item->parent->extra_expanded) { + item->parent->extra_expanded = true; + beginInsertRows(index.parent(), 7, 13); + endInsertRows(); + } else { + item->parent->extra_expanded = false; + beginRemoveRows(index.parent(), 7, 13); + endRemoveRows(); + } + } +} + +bool SignalModel::saveSignal(const Signal *origin_s, Signal &s) { + auto msg = dbc()->msg(msg_id); + if (s.name != origin_s->name && msg->sigs.count(s.name.c_str()) != 0) { + QString text = tr("There is already a signal with the same name '%1'").arg(s.name.c_str()); + QMessageBox::warning(nullptr, tr("Failed to save signal"), text); + return false; + } + + if (s.is_little_endian != origin_s->is_little_endian) { int start = std::floor(s.start_bit / 8); - if (little_endian) { + if (s.is_little_endian) { int end = std::floor((s.start_bit - s.size + 1) / 8); s.start_bit = start == end ? s.start_bit - s.size + 1 : bigEndianStartBitsIndex(s.start_bit); } else { int end = std::floor((s.start_bit + s.size - 1) / 8); s.start_bit = start == end ? s.start_bit + s.size - 1 : bigEndianBitIndex(s.start_bit); } - s.is_little_endian = little_endian; } if (s.is_little_endian) { s.lsb = s.start_bit; @@ -161,44 +195,262 @@ void SignalEdit::saveSignal() { s.lsb = bigEndianStartBitsIndex(bigEndianBitIndex(s.start_bit) + s.size - 1); s.msb = s.start_bit; } - if (s != *sig) { - emit save(this->sig, s); + + UndoStack::push(new EditSignalCommand(msg_id, origin_s, s)); + return true; +} + +void SignalModel::addSignal(int start_bit, int size, bool little_endian) { + auto msg = dbc()->msg(msg_id); + for (int i = 1; !msg; ++i) { + QString name = QString("NEW_MSG_%1").arg(i); + if (std::none_of(dbc()->messages().begin(), dbc()->messages().end(), [&](auto &m) { return m.second.name == name; })) { + UndoStack::push(new EditMsgCommand(msg_id, name, can->lastMessage(msg_id).dat.size())); + msg = dbc()->msg(msg_id); + } + } + + Signal sig = {.is_little_endian = little_endian, .factor = 1}; + for (int i = 1; /**/; ++i) { + sig.name = "NEW_SIGNAL_" + std::to_string(i); + if (msg->sigs.count(sig.name.c_str()) == 0) break; } + updateSigSizeParamsFromRange(sig, start_bit, size); + UndoStack::push(new AddSigCommand(msg_id, sig)); +} + +void SignalModel::resizeSignal(const Signal *sig, int start_bit, int size) { + Signal s = *sig; + updateSigSizeParamsFromRange(s, start_bit, size); + saveSignal(sig, s); +} + +void SignalModel::removeSignal(const Signal *sig) { + UndoStack::push(new RemoveSigCommand(msg_id, sig)); } -void SignalEdit::setChartOpened(bool opened) { - plot_btn->setToolTip(opened ? tr("Close Plot") : tr("Show Plot\nSHIFT click to add to previous opened chart")); - plot_btn->setChecked(opened); +void SignalModel::handleMsgChanged(uint32_t address) { + if (address == DBCManager::parseId(msg_id).second) { + refresh(); + } } -void SignalEdit::updateForm(bool visible) { - if (visible && sig) { - if (form->name->text() != sig->name.c_str()) { - form->name->setText(sig->name.c_str()); +void SignalModel::handleSignalAdded(uint32_t address, const Signal *sig) { + if (address == DBCManager::parseId(msg_id).second) { + int i = 0; + for (; i < root->children.size(); ++i) { + if (sig->start_bit < root->children[i]->sig->start_bit) break; } - form->endianness->setCurrentIndex(sig->is_little_endian ? 0 : 1); - form->sign->setCurrentIndex(sig->is_signed ? 0 : 1); - form->factor->setText(QString::number(sig->factor)); - form->offset->setText(QString::number(sig->offset)); - form->msb->setText(QString::number(sig->msb)); - form->lsb->setText(QString::number(sig->lsb)); - form->size->setValue(sig->size); + beginInsertRows({}, i, i); + insertItem(root.get(), i, sig); + endInsertRows(); } - form->setVisible(visible); - icon->setText(visible ? "▼ " : "> "); } -void SignalEdit::signalHovered(const Signal *s) { - auto color = sig == s ? "white" : "black"; - color_label->setStyleSheet(QString("color:%1; background-color:%2").arg(color).arg(getColor(form_idx))); +void SignalModel::handleSignalUpdated(const Signal *sig) { + if (int row = signalRow(sig); row != -1) { + emit dataChanged(index(row, 0), index(row, 1), {Qt::DisplayRole, Qt::EditRole, Qt::CheckStateRole}); + } } -void SignalEdit::enterEvent(QEvent *event) { - emit highlight(sig); - QWidget::enterEvent(event); +void SignalModel::handleSignalRemoved(const Signal *sig) { + if (int row = signalRow(sig); row != -1) { + beginRemoveRows({}, row, row); + delete root->children.takeAt(row); + endRemoveRows(); + } +} + +// SignalItemDelegate + +SignalItemDelegate::SignalItemDelegate(QObject *parent) { + name_validator = new NameValidator(this); + double_validator = new QDoubleValidator(this); + small_font.setPointSize(8); + double_validator->setLocale(QLocale::C); // Match locale of QString::toDouble() instead of system +} + +void SignalItemDelegate::paint(QPainter *painter, const QStyleOptionViewItem &option, const QModelIndex &index) const { + auto item = (SignalModel::Item *)index.internalPointer(); + if (item && !index.parent().isValid() && index.column() == 0) { + painter->save(); + painter->setRenderHint(QPainter::Antialiasing); + if (option.state & QStyle::State_Selected) { + painter->fillRect(option.rect, option.palette.highlight()); + } + + // color label + auto bg_color = getColor(item->sig); + QRect rc{option.rect.left(), option.rect.top(), 18, option.rect.height()}; + painter->setPen(Qt::NoPen); + painter->setBrush(item->highlight ? bg_color.darker(125) : bg_color); + painter->drawRoundedRect(rc.adjusted(0, 2, 0, -2), 3, 3); + painter->setPen(item->highlight ? Qt::white : Qt::black); + painter->setFont(small_font); + painter->drawText(rc, Qt::AlignCenter, QString::number(item->row() + 1)); + + // signal name + painter->setFont(option.font); + painter->setPen((option.state & QStyle::State_Selected ? option.palette.highlightedText() : option.palette.text()).color()); + QString text = index.data(Qt::DisplayRole).toString(); + QRect text_rect = option.rect.adjusted(rc.width() + 6, 0, 0, 0); + text = painter->fontMetrics().elidedText(text, Qt::ElideRight, text_rect.width()); + painter->drawText(text_rect, option.displayAlignment, text); + painter->restore(); + } else { + QStyledItemDelegate::paint(painter, option, index); + } +} + +QWidget *SignalItemDelegate::createEditor(QWidget *parent, const QStyleOptionViewItem &option, const QModelIndex &index) const { + auto item = (SignalModel::Item *)index.internalPointer(); + if (item->type == SignalModel::Item::Name || item->type == SignalModel::Item::Offset || item->type == SignalModel::Item::Factor) { + QLineEdit *e = new QLineEdit(parent); + e->setFrame(false); + e->setValidator(index.row() == 0 ? name_validator : double_validator); + return e; + } else if (item->type == SignalModel::Item::Size) { + QSpinBox *spin = new QSpinBox(parent); + spin->setFrame(false); + spin->setRange(1, 64); + return spin; + } + return QStyledItemDelegate::createEditor(parent, option, index); +} + +// SignalView + +SignalView::SignalView(ChartsWidget *charts, QWidget *parent) : charts(charts), QWidget(parent) { + // title bar + QWidget *title_bar = new QWidget(this); + title_bar->setAutoFillBackground(true); + QHBoxLayout *hl = new QHBoxLayout(title_bar); + hl->addWidget(signal_count_lb = new QLabel()); + filter_edit = new QLineEdit(this); + filter_edit->setClearButtonEnabled(true); + filter_edit->setPlaceholderText(tr("filter signals by name")); + hl->addWidget(filter_edit); + hl->addStretch(1); + auto collapse_btn = new QToolButton(); + collapse_btn->setIcon(utils::icon("dash-square")); + collapse_btn->setIconSize({12, 12}); + collapse_btn->setAutoRaise(true); + collapse_btn->setToolTip(tr("Collapse All")); + hl->addWidget(collapse_btn); + + // tree view + tree = new QTreeView(this); + tree->setModel(model = new SignalModel(this)); + tree->setItemDelegate(new SignalItemDelegate(this)); + tree->setFrameShape(QFrame::NoFrame); + tree->setHeaderHidden(true); + tree->setMouseTracking(true); + tree->setExpandsOnDoubleClick(false); + tree->header()->setSectionResizeMode(QHeaderView::Stretch); + tree->setMinimumHeight(300); + tree->setStyleSheet("QSpinBox{background-color:white;border:none;} QLineEdit{background-color:white;}"); + + QVBoxLayout *main_layout = new QVBoxLayout(this); + main_layout->setContentsMargins(0, 0, 0, 0); + main_layout->setSpacing(0); + main_layout->addWidget(title_bar); + main_layout->addWidget(tree); + + QObject::connect(filter_edit, &QLineEdit::textEdited, model, &SignalModel::setFilter); + QObject::connect(collapse_btn, &QPushButton::clicked, tree, &QTreeView::collapseAll); + QObject::connect(tree, &QAbstractItemView::clicked, this, &SignalView::rowClicked); + QObject::connect(tree, &QTreeView::viewportEntered, [this]() { emit highlight(nullptr); }); + QObject::connect(tree, &QTreeView::entered, [this](const QModelIndex &index) { emit highlight(model->getItem(index)->sig); }); + QObject::connect(model, &QAbstractItemModel::modelReset, this, &SignalView::rowsChanged); + QObject::connect(model, &QAbstractItemModel::rowsInserted, this, &SignalView::rowsChanged); + QObject::connect(model, &QAbstractItemModel::rowsRemoved, this, &SignalView::rowsChanged); + QObject::connect(dbc(), &DBCManager::signalAdded, [this](uint32_t address, const Signal *sig) { expandSignal(sig); }); +} + +void SignalView::setMessage(const QString &id) { + msg_id = id; + filter_edit->clear(); + model->setMessage(id); +} + +void SignalView::rowsChanged() { + auto create_btn = [](const QString &id, const QString &tooltip) { + auto btn = new QToolButton(); + btn->setIcon(utils::icon(id)); + btn->setToolTip(tooltip); + btn->setAutoRaise(true); + return btn; + }; + + signal_count_lb->setText(tr("Signals: %1").arg(model->rowCount())); + + for (int i = 0; i < model->rowCount(); ++i) { + auto index = model->index(i, 1); + if (!tree->indexWidget(index)) { + QWidget *w = new QWidget(this); + QHBoxLayout *h = new QHBoxLayout(w); + h->setContentsMargins(0, 2, 0, 2); + h->addStretch(1); + + auto remove_btn = create_btn("x", tr("Remove signal")); + auto plot_btn = create_btn("graph-up", ""); + plot_btn->setCheckable(true); + h->addWidget(plot_btn); + h->addWidget(remove_btn); + + tree->setIndexWidget(index, w); + auto sig = model->getItem(index)->sig; + QObject::connect(remove_btn, &QToolButton::clicked, [=]() { model->removeSignal(sig); }); + QObject::connect(plot_btn, &QToolButton::clicked, [=](bool checked) { + emit showChart(msg_id, sig, checked, QGuiApplication::keyboardModifiers() & Qt::ShiftModifier); + }); + } + } + updateChartState(); +} + +void SignalView::rowClicked(const QModelIndex &index) { + auto item = model->getItem(index); + if (item->type == SignalModel::Item::Sig) { + auto sig_index = model->index(index.row(), 0, index.parent()); + tree->setExpanded(sig_index, !tree->isExpanded(sig_index)); + } else if (item->type == SignalModel::Item::ExtraInfo) { + model->showExtraInfo(index); + } +} + +void SignalView::expandSignal(const Signal *sig) { + if (int row = model->signalRow(sig); row != -1) { + auto idx = model->index(row, 0); + bool expand = !tree->isExpanded(idx); + tree->setExpanded(idx, expand); + tree->scrollTo(idx, QAbstractItemView::PositionAtTop); + if (expand) tree->setCurrentIndex(idx); + } +} + +void SignalView::updateChartState() { + int i = 0; + for (auto item : model->root->children) { + auto plot_btn = tree->indexWidget(model->index(i, 1))->findChildren()[0]; + bool chart_opened = charts->hasSignal(msg_id, item->sig); + plot_btn->setChecked(chart_opened); + plot_btn->setToolTip(chart_opened ? tr("Close Plot") : tr("Show Plot\nSHIFT click to add to previous opened plot")); + ++i; + } +} + +void SignalView::signalHovered(const Signal *sig) { + auto &children = model->root->children; + for (int i = 0; i < children.size(); ++i) { + bool highlight = children[i]->sig == sig; + if (std::exchange(children[i]->highlight, highlight) != highlight) { + emit model->dataChanged(model->index(i, 0), model->index(i, 0)); + } + } } -void SignalEdit::leaveEvent(QEvent *event) { +void SignalView::leaveEvent(QEvent *event) { emit highlight(nullptr); QWidget::leaveEvent(event); } diff --git a/tools/cabana/signaledit.h b/tools/cabana/signaledit.h index d7b9084e7f..c0b649209a 100644 --- a/tools/cabana/signaledit.h +++ b/tools/cabana/signaledit.h @@ -1,58 +1,99 @@ #pragma once -#include +#include #include #include -#include -#include +#include +#include -#include "selfdrive/ui/qt/widgets/controls.h" -#include "tools/cabana/canmessages.h" +#include "tools/cabana/chartswidget.h" #include "tools/cabana/dbcmanager.h" +#include "tools/cabana/streams/abstractstream.h" -class SignalForm : public QWidget { +class SignalModel : public QAbstractItemModel { Q_OBJECT public: - SignalForm(QWidget *parent); - void textBoxEditingFinished(); + struct Item { + enum Type {Root, Sig, Name, Size, Endian, Signed, Offset, Factor, ExtraInfo, Unit, Comment, Min, Max, Desc }; + ~Item() { qDeleteAll(children); } + inline int row() { return parent->children.indexOf(this); } - QLineEdit *name, *unit, *comment, *val_desc, *offset, *factor, *min_val, *max_val; - QLabel *lsb, *msb; - QSpinBox *size; - QComboBox *sign, *endianness; + Type type = Type::Root; + Item *parent = nullptr; + QList children; -signals: - void changed(); + const Signal *sig = nullptr; + QString title; + bool highlight = false; + bool extra_expanded = false; + QString sig_val = "-"; + }; + + SignalModel(QObject *parent); + int rowCount(const QModelIndex &parent = QModelIndex()) const override; + int columnCount(const QModelIndex &parent = QModelIndex()) const override { return 2; } + QVariant data(const QModelIndex &index, int role = Qt::DisplayRole) const override; + QModelIndex index(int row, int column, const QModelIndex &parent = QModelIndex()) const override; + QModelIndex parent(const QModelIndex &index) const override; + Qt::ItemFlags flags(const QModelIndex &index) const override; + bool setData(const QModelIndex &index, const QVariant &value, int role = Qt::EditRole) override; + void setMessage(const QString &id); + void setFilter(const QString &txt); + void addSignal(int start_bit, int size, bool little_endian); + bool saveSignal(const Signal *origin_s, Signal &s); + void resizeSignal(const Signal *sig, int start_bit, int size); + void removeSignal(const Signal *sig); + inline Item *getItem(const QModelIndex &index) const { return index.isValid() ? (Item *)index.internalPointer() : root.get(); } + int signalRow(const Signal *sig) const; + void showExtraInfo(const QModelIndex &index); + +private: + void insertItem(SignalModel::Item *parent_item, int pos, const Signal *sig); + void handleSignalAdded(uint32_t address, const Signal *sig); + void handleSignalUpdated(const Signal *sig); + void handleSignalRemoved(const Signal *sig); + void handleMsgChanged(uint32_t address); + void refresh(); + void updateState(const QHash *msgs); + + QString msg_id; + QString filter_str; + std::unique_ptr root; + friend class SignalView; +}; + +class SignalItemDelegate : public QStyledItemDelegate { +public: + SignalItemDelegate(QObject *parent); + void paint(QPainter *painter, const QStyleOptionViewItem &option, const QModelIndex &index) const override; + QWidget *createEditor(QWidget *parent, const QStyleOptionViewItem &option, const QModelIndex &index) const override; + QValidator *name_validator, *double_validator; + QFont small_font; }; -class SignalEdit : public QWidget { +class SignalView : public QWidget { Q_OBJECT public: - SignalEdit(int index, QWidget *parent = nullptr); - void setSignal(const QString &msg_id, const Signal *sig); - void setChartOpened(bool opened); + SignalView(ChartsWidget *charts, QWidget *parent); + void setMessage(const QString &id); void signalHovered(const Signal *sig); - void updateForm(bool show); - const Signal *sig = nullptr; - SignalForm *form = nullptr; - QString msg_id; + void updateChartState(); + void expandSignal(const Signal *sig); + void rowClicked(const QModelIndex &index); + SignalModel *model = nullptr; signals: void highlight(const Signal *sig); void showChart(const QString &name, const Signal *sig, bool show, bool merge); - void remove(const Signal *sig); - void save(const Signal *sig, const Signal &new_sig); - void showFormClicked(const Signal *sig); - -protected: - void enterEvent(QEvent *event) override; - void leaveEvent(QEvent *event) override; - void saveSignal(); - - ElidedLabel *title; - QLabel *color_label; - QLabel *icon; - int form_idx = 0; - QToolButton *plot_btn; + +private: + void rowsChanged(); + void leaveEvent(QEvent *event); + + QString msg_id; + QTreeView *tree; + QLineEdit *filter_edit; + ChartsWidget *charts; + QLabel *signal_count_lb; }; diff --git a/tools/cabana/streams/abstractstream.cc b/tools/cabana/streams/abstractstream.cc new file mode 100644 index 0000000000..13b154a7ea --- /dev/null +++ b/tools/cabana/streams/abstractstream.cc @@ -0,0 +1,96 @@ +#include "tools/cabana/streams/abstractstream.h" + +AbstractStream *can = nullptr; + +AbstractStream::AbstractStream(QObject *parent, bool is_live_streaming) : is_live_streaming(is_live_streaming), QObject(parent) { + can = this; + new_msgs = std::make_unique>(); + QObject::connect(this, &AbstractStream::received, this, &AbstractStream::process, Qt::QueuedConnection); + QObject::connect(this, &AbstractStream::seekedTo, this, &AbstractStream::updateLastMsgsTo); +} + +void AbstractStream::process(QHash *messages) { + for (auto it = messages->begin(); it != messages->end(); ++it) { + can_msgs[it.key()] = it.value(); + } + emit updated(); + emit msgsReceived(messages); + delete messages; + processing = false; +} + +bool AbstractStream::updateEvent(const Event *event) { + static double prev_update_ts = 0; + + if (event->which == cereal::Event::Which::CAN) { + double current_sec = event->mono_time / 1e9 - routeStartTime(); + for (const auto &c : event->event.getCan()) { + QString id = QString("%1:%2").arg(c.getSrc()).arg(c.getAddress(), 1, 16); + CanData &data = (*new_msgs)[id]; + data.ts = current_sec; + data.dat = QByteArray((char *)c.getDat().begin(), c.getDat().size()); + data.count = ++counters[id]; + data.freq = data.count / std::max(1.0, current_sec); + change_trackers[id].compute(data.dat, data.ts, data.freq); + data.colors = change_trackers[id].colors; + data.last_change_t = change_trackers[id].last_change_t; + data.bit_change_counts = change_trackers[id].bit_change_counts; + } + + double ts = millis_since_boot(); + if ((ts - prev_update_ts) > (1000.0 / settings.fps) && !processing && !new_msgs->isEmpty()) { + // delay posting CAN message if UI thread is busy + processing = true; + prev_update_ts = ts; + // use pointer to avoid data copy in queued connection. + emit received(new_msgs.release()); + new_msgs.reset(new QHash); + new_msgs->reserve(100); + } + } + return true; +} + +const CanData &AbstractStream::lastMessage(const QString &id) { + static CanData empty_data; + auto it = can_msgs.find(id); + return it != can_msgs.end() ? it.value() : empty_data; +} + +void AbstractStream::updateLastMsgsTo(double sec) { + QHash, CanData> last_msgs; // Much faster than QHash + last_msgs.reserve(can_msgs.size()); + double route_start_time = routeStartTime(); + uint64_t last_ts = (sec + route_start_time) * 1e9; + auto last = std::upper_bound(events()->rbegin(), events()->rend(), last_ts, [](uint64_t ts, auto &e) { return e->mono_time < ts; }); + for (auto it = last; it != events()->rend(); ++it) { + if ((*it)->which == cereal::Event::Which::CAN) { + for (const auto &c : (*it)->event.getCan()) { + auto &m = last_msgs[{c.getSrc(), c.getAddress()}]; + if (++m.count == 1) { + m.ts = ((*it)->mono_time / 1e9) - route_start_time; + m.dat = QByteArray((char *)c.getDat().begin(), c.getDat().size()); + m.colors = QVector(m.dat.size(), QColor(0, 0, 0, 0)); + m.last_change_t = QVector(m.dat.size(), m.ts); + m.bit_change_counts.resize(m.dat.size()); + } else { + m.freq = m.count / std::max(1.0, m.ts); + } + } + } + } + + // it is thread safe to update data here. + // updateEvent will not be called before replayStream::seekedTo return. + new_msgs->clear(); + change_trackers.clear(); + counters.clear(); + can_msgs.clear(); + for (auto it = last_msgs.cbegin(); it != last_msgs.cend(); ++it) { + QString msg_id = QString("%1:%2").arg(it.key().first).arg(it.key().second, 1, 16); + can_msgs[msg_id] = it.value(); + counters[msg_id] = it.value().count; + } + emit updated(); + emit msgsReceived(&can_msgs); +} diff --git a/tools/cabana/streams/abstractstream.h b/tools/cabana/streams/abstractstream.h new file mode 100644 index 0000000000..8c10d959cb --- /dev/null +++ b/tools/cabana/streams/abstractstream.h @@ -0,0 +1,71 @@ +#pragma once + +#include + +#include +#include + +#include "tools/cabana/settings.h" +#include "tools/cabana/util.h" +#include "tools/replay/replay.h" + +struct CanData { + double ts = 0.; + uint32_t count = 0; + uint32_t freq = 0; + QByteArray dat; + QVector colors; + QVector last_change_t; + QVector> bit_change_counts; +}; + +class AbstractStream : public QObject { + Q_OBJECT + +public: + AbstractStream(QObject *parent, bool is_live_streaming); + virtual ~AbstractStream() {}; + inline bool liveStreaming() const { return is_live_streaming; } + virtual void seekTo(double ts) {} + virtual QString routeName() const = 0; + virtual QString carFingerprint() const { return ""; } + virtual double totalSeconds() const { return 0; } + virtual double routeStartTime() const { return 0; } + virtual double currentSec() const = 0; + virtual QDateTime currentDateTime() const { return {}; } + virtual const CanData &lastMessage(const QString &id); + virtual VisionStreamType visionStreamType() const { return VISION_STREAM_ROAD; } + virtual const Route *route() const { return nullptr; } + virtual const std::vector *events() const = 0; + virtual void setSpeed(float speed) {} + virtual bool isPaused() const { return false; } + virtual void pause(bool pause) {} + virtual const std::vector> getTimeline() { return {}; } + +signals: + void paused(); + void resume(); + void seekedTo(double sec); + void streamStarted(); + void eventsMerged(); + void updated(); + void msgsReceived(const QHash *); + void received(QHash *); + +public: + QHash can_msgs; + +protected: + void process(QHash *); + bool updateEvent(const Event *event); + void updateLastMsgsTo(double sec); + + bool is_live_streaming = false; + std::atomic processing = false; + QHash counters; + std::unique_ptr> new_msgs; + QHash change_trackers; +}; + +// A global pointer referring to the unique AbstractStream object +extern AbstractStream *can; diff --git a/tools/cabana/streams/livestream.cc b/tools/cabana/streams/livestream.cc new file mode 100644 index 0000000000..b2fc7ea4a6 --- /dev/null +++ b/tools/cabana/streams/livestream.cc @@ -0,0 +1,102 @@ +#include "tools/cabana/streams/livestream.h" + +LiveStream::LiveStream(QObject *parent, QString address) : zmq_address(address), AbstractStream(parent, true) { + timer = new QTimer(this); + timer->callOnTimeout(this, &LiveStream::removeExpiredEvents); + timer->start(3 * 1000); + + stream_thread = new QThread(this); + QObject::connect(stream_thread, &QThread::started, [=]() { streamThread(); }); + QObject::connect(stream_thread, &QThread::finished, stream_thread, &QThread::deleteLater); + QTimer::singleShot(0, [this]() { stream_thread->start(); }); +} + +LiveStream::~LiveStream() { + stream_thread->requestInterruption(); + stream_thread->quit(); + stream_thread->wait(); + for (Event *e : can_events) ::delete e; + for (auto m : messages) delete m; +} + +void LiveStream::streamThread() { + if (!zmq_address.isEmpty()) { + setenv("ZMQ", "1", 1); + } + std::unique_ptr context(Context::create()); + std::string address = zmq_address.isEmpty() ? "127.0.0.1" : zmq_address.toStdString(); + std::unique_ptr sock(SubSocket::create(context.get(), "can", address)); + assert(sock != NULL); + sock->setTimeout(50); + // run as fast as messages come in + while (!QThread::currentThread()->isInterruptionRequested()) { + Message *msg = sock->receive(true); + if (!msg) { + QThread::msleep(50); + continue; + } + AlignedBuffer *buf = messages.emplace_back(new AlignedBuffer()); + Event *evt = ::new Event(buf->align(msg)); + delete msg; + + handleEvent(evt); + // TODO: write stream to log file to replay it with cabana --data_dir flag. + } +} + +void LiveStream::handleEvent(Event *evt) { + current_ts = evt->mono_time; + if (start_ts == 0 || current_ts < start_ts) { + if (current_ts < start_ts) { + qDebug() << "stream is looping back to old time stamp"; + } + start_ts = current_ts.load(); + emit streamStarted(); + } + + std::lock_guard lk(lock); + can_events.push_back(evt); + if (!pause_) { + if (speed_ < 1 && last_update_ts > 0) { + auto it = std::upper_bound(can_events.begin(), can_events.end(), last_update_event_ts, [](uint64_t ts, auto &e) { + return ts < e->mono_time; + }); + + if (it != can_events.end() && + (nanos_since_boot() - last_update_ts) < ((*it)->mono_time - last_update_event_ts) / speed_) { + return; + } + evt = (*it); + } + updateEvent(evt); + last_update_event_ts = evt->mono_time; + last_update_ts = nanos_since_boot(); + } +} + +void LiveStream::removeExpiredEvents() { + std::lock_guard lk(lock); + if (can_events.size() > 0) { + const uint64_t max_ns = settings.max_cached_minutes * 60 * 1e9; + const uint64_t last_ns = can_events.back()->mono_time; + while (!can_events.empty() && (last_ns - can_events.front()->mono_time) > max_ns) { + ::delete can_events.front(); + delete messages.front(); + can_events.pop_front(); + messages.pop_front(); + } + } +} + +const std::vector *LiveStream::events() const { + events_vector.clear(); + std::lock_guard lk(lock); + events_vector.reserve(can_events.size()); + std::copy(can_events.begin(), can_events.end(), std::back_inserter(events_vector)); + return &events_vector; +} + +void LiveStream::pause(bool pause) { + pause_ = pause; + emit paused(); +} diff --git a/tools/cabana/streams/livestream.h b/tools/cabana/streams/livestream.h new file mode 100644 index 0000000000..ba20032b67 --- /dev/null +++ b/tools/cabana/streams/livestream.h @@ -0,0 +1,41 @@ +#pragma once + +#include +#include "tools/cabana/streams/abstractstream.h" + +class LiveStream : public AbstractStream { + Q_OBJECT + +public: + LiveStream(QObject *parent, QString address = {}); + virtual ~LiveStream(); + inline QString routeName() const override { + return QString("Live Streaming From %1").arg(zmq_address.isEmpty() ? "127.0.0.1" : zmq_address); + } + inline double routeStartTime() const override { return start_ts / (double)1e9; } + inline double currentSec() const override { return (current_ts - start_ts) / (double)1e9; } + void setSpeed(float speed) override { speed_ = std::min(1.0, speed); } + bool isPaused() const override { return pause_; } + void pause(bool pause) override; + const std::vector *events() const override; + +protected: + virtual void handleEvent(Event *evt); + virtual void streamThread(); + virtual void removeExpiredEvents(); + + mutable std::mutex lock; + mutable std::vector events_vector; + std::deque can_events; + std::deque messages; + std::atomic start_ts = 0; + std::atomic current_ts = 0; + std::atomic speed_ = 1; + std::atomic pause_ = false; + uint64_t last_update_event_ts = 0; + uint64_t last_update_ts = 0; + + const QString zmq_address; + QThread *stream_thread; + QTimer *timer; +}; diff --git a/tools/cabana/streams/replaystream.cc b/tools/cabana/streams/replaystream.cc new file mode 100644 index 0000000000..b768b94327 --- /dev/null +++ b/tools/cabana/streams/replaystream.cc @@ -0,0 +1,48 @@ +#include "tools/cabana/streams/replaystream.h" + +#include "tools/cabana/dbcmanager.h" + +ReplayStream::ReplayStream(uint32_t replay_flags, QObject *parent) : replay_flags(replay_flags), AbstractStream(parent, false) { + QObject::connect(&settings, &Settings::changed, [this]() { + if (replay) replay->setSegmentCacheLimit(settings.max_cached_minutes); + }); +} + +ReplayStream::~ReplayStream() { + if (replay) replay->stop(); +} + +static bool event_filter(const Event *e, void *opaque) { + return ((ReplayStream *)opaque)->eventFilter(e); +} + +bool ReplayStream::loadRoute(const QString &route, const QString &data_dir) { + replay.reset(new Replay(route, {"can", "roadEncodeIdx", "wideRoadEncodeIdx", "carParams"}, {}, nullptr, replay_flags, data_dir, this)); + replay->setSegmentCacheLimit(settings.max_cached_minutes); + replay->installEventFilter(event_filter, this); + QObject::connect(replay.get(), &Replay::seekedTo, this, &AbstractStream::seekedTo); + QObject::connect(replay.get(), &Replay::segmentsMerged, this, &AbstractStream::eventsMerged); + QObject::connect(replay.get(), &Replay::streamStarted, this, &AbstractStream::streamStarted); + if (replay->load()) { + const auto &segments = replay->route()->segments(); + if (std::none_of(segments.begin(), segments.end(), [](auto &s) { return s.second.rlog.length() > 0; })) { + qWarning() << "no rlogs in route" << route; + return false; + } + replay->start(); + return true; + } + return false; +} + +bool ReplayStream::eventFilter(const Event *event) { + if (event->which == cereal::Event::Which::CAN) { + updateEvent(event); + } + return true; +} + +void ReplayStream::pause(bool pause) { + replay->pause(pause); + emit(pause ? paused() : resume()); +} diff --git a/tools/cabana/streams/replaystream.h b/tools/cabana/streams/replaystream.h new file mode 100644 index 0000000000..69fb738ab8 --- /dev/null +++ b/tools/cabana/streams/replaystream.h @@ -0,0 +1,33 @@ +#pragma once + +#include "opendbc/can/common_dbc.h" +#include "tools/cabana/streams/abstractstream.h" +#include "tools/cabana/settings.h" + +class ReplayStream : public AbstractStream { + Q_OBJECT + +public: + ReplayStream(uint32_t replay_flags, QObject *parent); + ~ReplayStream(); + bool loadRoute(const QString &route, const QString &data_dir); + bool eventFilter(const Event *event); + void seekTo(double ts) override { replay->seekTo(std::max(double(0), ts), false); }; + inline QString routeName() const override { return replay->route()->name(); } + inline QString carFingerprint() const override { return replay->carFingerprint().c_str(); } + inline VisionStreamType visionStreamType() const override { return replay->hasFlag(REPLAY_FLAG_ECAM) ? VISION_STREAM_WIDE_ROAD : VISION_STREAM_ROAD; } + inline double totalSeconds() const override { return replay->totalSeconds(); } + inline double routeStartTime() const override { return replay->routeStartTime() / (double)1e9; } + inline double currentSec() const override { return replay->currentSeconds(); } + inline QDateTime currentDateTime() const override { return replay->currentDateTime(); } + inline const Route *route() const override { return replay->route(); } + inline const std::vector *events() const override { return replay->events(); } + inline void setSpeed(float speed) override { replay->setSpeed(speed); } + inline bool isPaused() const override { return replay->isPaused(); } + void pause(bool pause) override; + inline const std::vector> getTimeline() override { return replay->getTimeline(); } + +private: + std::unique_ptr replay = nullptr; + uint32_t replay_flags = REPLAY_FLAG_NONE; +}; diff --git a/tools/cabana/tools/findsimilarbits.cc b/tools/cabana/tools/findsimilarbits.cc index 8a05e3e236..63d01b152d 100644 --- a/tools/cabana/tools/findsimilarbits.cc +++ b/tools/cabana/tools/findsimilarbits.cc @@ -7,8 +7,8 @@ #include #include -#include "tools/cabana/canmessages.h" #include "tools/cabana/dbcmanager.h" +#include "tools/cabana/streams/abstractstream.h" FindSimilarBitsDlg::FindSimilarBitsDlg(QWidget *parent) : QDialog(parent, Qt::WindowFlags() | Qt::Window) { setWindowTitle(tr("Find similar bits")); diff --git a/tools/cabana/util.cc b/tools/cabana/util.cc new file mode 100644 index 0000000000..454dd29b87 --- /dev/null +++ b/tools/cabana/util.cc @@ -0,0 +1,148 @@ +#include "tools/cabana/util.h" + +#include +#include +#include +#include + +#include +#include + +#include "selfdrive/ui/qt/util.h" + +static QColor blend(QColor a, QColor b) { + return QColor((a.red() + b.red()) / 2, (a.green() + b.green()) / 2, (a.blue() + b.blue()) / 2, (a.alpha() + b.alpha()) / 2); +} + +void ChangeTracker::compute(const QByteArray &dat, double ts, uint32_t freq) { + if (prev_dat.size() != dat.size()) { + colors.resize(dat.size()); + last_change_t.resize(dat.size()); + bit_change_counts.resize(dat.size()); + std::fill(colors.begin(), colors.end(), QColor(0, 0, 0, 0)); + std::fill(last_change_t.begin(), last_change_t.end(), ts); + } else { + for (int i = 0; i < dat.size(); ++i) { + const uint8_t last = prev_dat[i]; + const uint8_t cur = dat[i]; + + if (last != cur) { + double delta_t = ts - last_change_t[i]; + if (delta_t * freq > periodic_threshold) { + // Last change was while ago, choose color based on delta up or down + if (cur > last) { + colors[i] = QColor(0, 187, 255, start_alpha); // Cyan + } else { + colors[i] = QColor(255, 0, 0, start_alpha); // Red + } + } else { + // Periodic changes + colors[i] = blend(colors[i], QColor(102, 86, 169, start_alpha / 2)); // Greyish/Blue + } + + // Track bit level changes + for (int bit = 0; bit < 8; bit++){ + if ((cur ^ last) & (1 << bit)) { + bit_change_counts[i][bit] += 1; + } + } + + last_change_t[i] = ts; + } else { + // Fade out + float alpha_delta = 1.0 / (freq + 1) / fade_time; + colors[i].setAlphaF(std::max(0.0, colors[i].alphaF() - alpha_delta)); + } + } + } + + prev_dat = dat; +} + +void ChangeTracker::clear() { + prev_dat.clear(); + last_change_t.clear(); + bit_change_counts.clear(); + colors.clear(); +} + +QList ChangeTracker::toVariantList(const QVector &colors) { + QList ret; + ret.reserve(colors.size()); + for (auto &c : colors) ret.append(c); + return ret; +} + +// MessageBytesDelegate + +MessageBytesDelegate::MessageBytesDelegate(QObject *parent) : QStyledItemDelegate(parent) { + fixed_font = QFontDatabase::systemFont(QFontDatabase::FixedFont); +} + +void MessageBytesDelegate::paint(QPainter *painter, const QStyleOptionViewItem &option, const QModelIndex &index) const { + QStyleOptionViewItemV4 opt = option; + initStyleOption(&opt, index); + + auto byte_list = opt.text.split(" "); + if (byte_list.size() <= 1) { + QStyledItemDelegate::paint(painter, option, index); + return; + } + + if ((option.state & QStyle::State_Selected) && (option.state & QStyle::State_Active)) { + painter->setPen(option.palette.color(QPalette::HighlightedText)); + } else { + painter->setPen(option.palette.color(QPalette::Text)); + } + + painter->setFont(fixed_font); + QRect space = painter->boundingRect(opt.rect, opt.displayAlignment, " "); + QRect pos = painter->boundingRect(opt.rect, opt.displayAlignment, "00"); + pos.moveLeft(pos.x() + space.width()); + + int m = space.width() / 2; + const QMargins margins(m, m, m, m); + + QList colors = index.data(Qt::UserRole).toList(); + int i = 0; + for (auto &byte : byte_list) { + if (i < colors.size()) { + painter->fillRect(pos.marginsAdded(margins), colors[i].value()); + } + painter->drawText(pos, opt.displayAlignment, byte); + pos.moveLeft(pos.right() + space.width()); + i++; + } +} + +QColor getColor(const Signal *sig) { + float h = 19 * (float)sig->lsb / 64.0; + h = fmod(h, 1.0); + + size_t hash = qHash(QString::fromStdString(sig->name)); + float s = 0.25 + 0.25 * (float)(hash & 0xff) / 255.0; + float v = 0.75 + 0.25 * (float)((hash >> 8) & 0xff) / 255.0; + + return QColor::fromHsvF(h, s, v); +} + +NameValidator::NameValidator(QObject *parent) : QRegExpValidator(QRegExp("^(\\w+)"), parent) { } + +QValidator::State NameValidator::validate(QString &input, int &pos) const { + input.replace(' ', '_'); + return QRegExpValidator::validate(input, pos); +} + +namespace utils { +QPixmap icon(const QString &id) { + static bool dark_theme = QApplication::style()->standardPalette().color(QPalette::WindowText).value() > + QApplication::style()->standardPalette().color(QPalette::Background).value(); + QPixmap pm = bootstrapPixmap(id); + if (dark_theme) { + QPainter p(&pm); + p.setCompositionMode(QPainter::CompositionMode_SourceIn); + p.fillRect(pm.rect(), Qt::lightGray); + } + return pm; +} +} // namespace utils diff --git a/tools/cabana/util.h b/tools/cabana/util.h new file mode 100644 index 0000000000..8ec4cda90c --- /dev/null +++ b/tools/cabana/util.h @@ -0,0 +1,53 @@ +#pragma once + +#include + +#include +#include +#include +#include +#include +#include + +#include "opendbc/can/common_dbc.h" + +class ChangeTracker { +public: + void compute(const QByteArray &dat, double ts, uint32_t freq); + static QList toVariantList(const QVector &colors); + void clear(); + + QVector last_change_t; + QVector colors; + QVector> bit_change_counts; + +private: + const int periodic_threshold = 10; + const int start_alpha = 128; + const float fade_time = 2.0; + QByteArray prev_dat; +}; + +class MessageBytesDelegate : public QStyledItemDelegate { + Q_OBJECT +public: + MessageBytesDelegate(QObject *parent); + void paint(QPainter *painter, const QStyleOptionViewItem &option, const QModelIndex &index) const override; + QFont fixed_font; +}; + +inline QString toHex(const QByteArray &dat) { return dat.toHex(' ').toUpper(); } +inline char toHex(uint value) { return "0123456789ABCDEF"[value & 0xF]; } +QColor getColor(const Signal *sig); + +class NameValidator : public QRegExpValidator { + Q_OBJECT + +public: + NameValidator(QObject *parent=nullptr); + QValidator::State validate(QString &input, int &pos) const override; +}; + +namespace utils { +QPixmap icon(const QString &id); +} diff --git a/tools/cabana/videowidget.cc b/tools/cabana/videowidget.cc index ed4354ce65..cd3dc0b516 100644 --- a/tools/cabana/videowidget.cc +++ b/tools/cabana/videowidget.cc @@ -3,11 +3,11 @@ #include #include #include -#include #include #include #include #include +#include #include #include #include @@ -17,43 +17,29 @@ inline QString formatTime(int seconds) { return QDateTime::fromTime_t(seconds).toString(seconds > 60 * 60 ? "hh:mm:ss" : "mm:ss"); } -VideoWidget::VideoWidget(QWidget *parent) : QFrame(parent) { - setFrameShape(QFrame::StyledPanel); - setFrameShadow(QFrame::Sunken); - QHBoxLayout *containter_layout = new QHBoxLayout(this); - QVBoxLayout *main_layout = new QVBoxLayout(); - main_layout->setContentsMargins(0, 0, 0, 0); +VideoWidget::VideoWidget(QWidget *parent) : QWidget(parent) { + QVBoxLayout *main_layout = new QVBoxLayout(this); + QFrame *frame = new QFrame(this); + frame->setFrameShape(QFrame::StyledPanel); + frame->setFrameShadow(QFrame::Sunken); + main_layout->addWidget(frame); - containter_layout->addStretch(1); - containter_layout->addLayout(main_layout); - containter_layout->addStretch(1); - - cam_widget = new CameraWidget("camerad", can->visionStreamType(), false, this); - cam_widget->setFixedSize(parent->width(), parent->width() / 1.596); - main_layout->addWidget(cam_widget); - - // slider controls - QHBoxLayout *slider_layout = new QHBoxLayout(); - QLabel *time_label = new QLabel("00:00"); - slider_layout->addWidget(time_label); - - slider = new Slider(this); - slider->setSingleStep(0); - slider_layout->addWidget(slider); - - end_time_label = new QLabel(this); - slider_layout->addWidget(end_time_label); - main_layout->addLayout(slider_layout); + QVBoxLayout *frame_layout = new QVBoxLayout(frame); + if (!can->liveStreaming()) { + frame_layout->addWidget(createCameraWidget()); + } // btn controls QHBoxLayout *control_layout = new QHBoxLayout(); - play_btn = new QPushButton("⏸"); - play_btn->setStyleSheet("font-weight:bold; height:16px"); + play_btn = new QPushButton(); + play_btn->setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Preferred); control_layout->addWidget(play_btn); QButtonGroup *group = new QButtonGroup(this); group->setExclusive(true); for (float speed : {0.1, 0.5, 1., 2.}) { + if (can->liveStreaming() && speed > 1) continue; + QPushButton *btn = new QPushButton(QString("%1x").arg(speed), this); btn->setCheckable(true); QObject::connect(btn, &QPushButton::clicked, [=]() { can->setSpeed(speed); }); @@ -61,22 +47,74 @@ VideoWidget::VideoWidget(QWidget *parent) : QFrame(parent) { group->addButton(btn); if (speed == 1.0) btn->setChecked(true); } - main_layout->addLayout(control_layout); + frame_layout->addLayout(control_layout); + + QObject::connect(play_btn, &QPushButton::clicked, []() { can->pause(!can->isPaused()); }); + QObject::connect(can, &AbstractStream::paused, this, &VideoWidget::updatePlayBtnState); + QObject::connect(can, &AbstractStream::resume, this, &VideoWidget::updatePlayBtnState); + updatePlayBtnState(); +} +QWidget *VideoWidget::createCameraWidget() { + QWidget *w = new QWidget(this); + QVBoxLayout *l = new QVBoxLayout(w); + l->setContentsMargins(0, 0, 0, 0); + cam_widget = new CameraWidget("camerad", can->visionStreamType(), false); + l->addWidget(cam_widget); + + // slider controls + slider_layout = new QHBoxLayout(); + time_label = new ElidedLabel("00:00"); + time_label->setToolTip(tr("Click to set current time")); + slider_layout->addWidget(time_label); + + slider = new Slider(this); + slider->setSingleStep(0); + slider_layout->addWidget(slider); + + end_time_label = new QLabel(this); + slider_layout->addWidget(end_time_label); + l->addLayout(slider_layout); + + cam_widget->setMinimumHeight(100); + cam_widget->setSizePolicy(QSizePolicy::Preferred, QSizePolicy::MinimumExpanding); + setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Maximum); + + QObject::connect(time_label, &ElidedLabel::clicked, this, &VideoWidget::timeLabelClicked); QObject::connect(slider, &QSlider::sliderReleased, [this]() { can->seekTo(slider->value() / 1000.0); }); QObject::connect(slider, &QSlider::valueChanged, [=](int value) { time_label->setText(formatTime(value / 1000)); }); QObject::connect(cam_widget, &CameraWidget::clicked, []() { can->pause(!can->isPaused()); }); - QObject::connect(play_btn, &QPushButton::clicked, []() { can->pause(!can->isPaused()); }); - QObject::connect(can, &CANMessages::updated, this, &VideoWidget::updateState); - QObject::connect(can, &CANMessages::paused, [this]() { play_btn->setText("▶"); }); - QObject::connect(can, &CANMessages::resume, [this]() { play_btn->setText("⏸"); }); - QObject::connect(can, &CANMessages::streamStarted, [this]() { + QObject::connect(can, &AbstractStream::updated, this, &VideoWidget::updateState); + QObject::connect(can, &AbstractStream::streamStarted, [this]() { end_time_label->setText(formatTime(can->totalSeconds())); slider->setRange(0, can->totalSeconds() * 1000); }); + return w; +} + +void VideoWidget::timeLabelClicked() { + auto time_edit = new QTimeEdit(this); + auto init_date_time = can->currentDateTime(); + time_edit->setDateTime(init_date_time); + time_edit->setDisplayFormat("hh:mm:ss"); + time_label->setVisible(false); + slider_layout->insertWidget(0, time_edit); + QTimer::singleShot(0, [=]() { time_edit->setFocus(); }); + + QObject::connect(time_edit, &QTimeEdit::editingFinished, [=]() { + if (time_edit->dateTime() != init_date_time) { + int seconds = can->route()->datetime().secsTo(time_edit->dateTime()); + can->seekTo(seconds); + } + time_edit->setVisible(false); + time_label->setVisible(true); + time_edit->deleteLater(); + }); } void VideoWidget::rangeChanged(double min, double max, bool is_zoomed) { + if (can->liveStreaming()) return; + if (!is_zoomed) { min = 0; max = can->totalSeconds(); @@ -90,6 +128,11 @@ void VideoWidget::updateState() { slider->setValue(can->currentSec() * 1000); } +void VideoWidget::updatePlayBtnState() { + play_btn->setIcon(utils::icon(can->isPaused() ? "play" : "pause")); + play_btn->setToolTip(can->isPaused() ? tr("Play") : tr("Pause")); +} + // Slider Slider::Slider(QWidget *parent) : QSlider(Qt::Horizontal, parent) { QTimer *timer = new QTimer(this); @@ -101,7 +144,12 @@ Slider::Slider(QWidget *parent) : QSlider(Qt::Horizontal, parent) { setMouseTracking(true); QObject::connect(can, SIGNAL(streamStarted()), timer, SLOT(start())); - QObject::connect(can, &CANMessages::streamStarted, this, &Slider::streamStarted); + QObject::connect(can, &AbstractStream::streamStarted, this, &Slider::streamStarted); +} + +Slider::~Slider() { + abort_load_thumbnail = true; + thumnail_future.waitForFinished(); } void Slider::streamStarted() { diff --git a/tools/cabana/videowidget.h b/tools/cabana/videowidget.h index ec8bc4bec4..51197eedd6 100644 --- a/tools/cabana/videowidget.h +++ b/tools/cabana/videowidget.h @@ -3,19 +3,22 @@ #include #include +#include #include #include #include #include #include "selfdrive/ui/qt/widgets/cameraview.h" -#include "tools/cabana/canmessages.h" +#include "selfdrive/ui/qt/widgets/controls.h" +#include "tools/cabana/streams/abstractstream.h" class Slider : public QSlider { Q_OBJECT public: Slider(QWidget *parent); + ~Slider(); private: void mousePressEvent(QMouseEvent *e) override; @@ -35,7 +38,7 @@ private: QSize thumbnail_size = {}; }; -class VideoWidget : public QFrame { +class VideoWidget : public QWidget { Q_OBJECT public: @@ -44,9 +47,14 @@ public: protected: void updateState(); + void updatePlayBtnState(); + void timeLabelClicked(); + QWidget *createCameraWidget(); CameraWidget *cam_widget; QLabel *end_time_label; + ElidedLabel *time_label; + QHBoxLayout *slider_layout; QPushButton *play_btn; Slider *slider; }; diff --git a/tools/gpstest/README.md b/tools/gpstest/README.md index 5aff0ee3d7..01f44df0ce 100644 --- a/tools/gpstest/README.md +++ b/tools/gpstest/README.md @@ -3,12 +3,9 @@ Testing the GPS receiver using GPS spoofing. At the moment only static location relpay is supported. # Usage -``` -# on host, start gps signal simulation -./run_static_lime.py -``` +on C3 run `rpc_server.py`, on host PC run `fuzzy_testing.py` -`run_static_lime.py` downloads the latest ephemeris file from +`simulate_gps_signal.py` downloads the latest ephemeris file from https://cddis.nasa.gov/archive/gnss/data/daily/20xx/brdc/. diff --git a/tools/gpstest/fuzzy_testing.py b/tools/gpstest/fuzzy_testing.py index bd204e7ae7..a2e130342c 100755 --- a/tools/gpstest/fuzzy_testing.py +++ b/tools/gpstest/fuzzy_testing.py @@ -1,147 +1,115 @@ #!/usr/bin/env python3 -import sys -import time -import random -import datetime as dt -import subprocess as sp +import argparse import multiprocessing -import threading -from typing import Tuple, Any +import rpyc # pylint: disable=import-error +from collections import defaultdict -from laika.downloader import download_nav -from laika.gps_time import GPSTime -from laika.helpers import ConstellationId +from helper import download_rinex, exec_LimeGPS_bin +from helper import get_random_coords, get_continuous_coords -cache_dir = '/tmp/gpstest/' +#------------------------------------------------------------------------------ +# this script is supposed to run on HOST PC +# limeSDR is unreliable via c3 USB +#------------------------------------------------------------------------------ -def download_rinex(): - # TODO: check if there is a better way to get the full brdc file for LimeGPS - gps_time = GPSTime.from_datetime(dt.datetime.utcnow()) - utc_time = dt.datetime.utcnow() - dt.timedelta(1) - gps_time = GPSTime.from_datetime(dt.datetime(utc_time.year, utc_time.month, utc_time.day)) - return download_nav(gps_time, cache_dir, ConstellationId.GPS) - - -def exec_LimeGPS_bin(rinex_file: str, location: str, duration: int): - # this functions should never return, cause return means, timeout is - # reached or it crashed - try: - cmd = ["LimeGPS/LimeGPS", "-e", rinex_file, "-l", location] - sp.check_output(cmd, timeout=duration) - except sp.TimeoutExpired: - print("LimeGPS timeout reached!") - except Exception as e: - print(f"LimeGPS crashed: {str(e)}") - - -def run_lime_gps(rinex_file: str, location: str, duration: int): - print(f"LimeGPS {location} {duration}") - +def run_lime_gps(rinex_file: str, location: str, timeout: int): + # needs to run longer than the checker + timeout += 10 + print(f"LimeGPS {location} {timeout}") p = multiprocessing.Process(target=exec_LimeGPS_bin, - args=(rinex_file, location, duration)) + args=(rinex_file, location, timeout)) p.start() return p +con = None +def run_remote_checker(lat, lon, alt, duration, ip_addr): + global con + try: + con = rpyc.connect(ip_addr, 18861) + con._config['sync_request_timeout'] = duration+20 + except ConnectionRefusedError: + print("could not run remote checker is 'rpc_server.py' running???") + return False, None, None -def get_random_coords(lat, lon) -> Tuple[int, int]: - # jump around the world - # max values, lat: -90 to 90, lon: -180 to 180 - - lat_add = random.random()*20 + 10 - lon_add = random.random()*20 + 20 - - lat = ((lat + lat_add + 90) % 180) - 90 - lon = ((lon + lon_add + 180) % 360) - 180 - return round(lat, 5), round(lon, 5) - -def get_continuous_coords(lat, lon) -> Tuple[int, int]: - # continuously move around the world - - lat_add = random.random()*0.01 - lon_add = random.random()*0.01 - - lat = ((lat + lat_add + 90) % 180) - 90 - lon = ((lon + lon_add + 180) % 360) - 180 - return round(lat, 5), round(lon, 5) - -rc_p: Any = None -def exec_remote_checker(lat, lon, duration, ip_addr): - global rc_p - # TODO: good enough for testing - remote_cmd = "export PYTHONPATH=/data/pythonpath && " - remote_cmd += "cd /data/openpilot && " - remote_cmd += f"timeout {duration} /usr/local/pyenv/shims/python tools/gpstest/remote_checker.py " - remote_cmd += f"{lat} {lon}" - - ssh_cmd = ["ssh", "-i", "/home/batman/openpilot/xx/phone/key/id_rsa", - f"comma@{ip_addr}"] - ssh_cmd += [remote_cmd] - - rc_p = sp.Popen(ssh_cmd, stdout=sp.PIPE) - rc_p.wait() - rc_output = rc_p.stdout.read() - print(f"Checker Result: {rc_output.strip().decode('utf-8')}") + matched, log, info = con.root.exposed_run_checker(lat, lon, alt, + timeout=duration, + use_laikad=True) + con.close() # TODO: might wanna fetch more logs here + con = None + print(f"Remote Checker: {log} {info}") + return matched, log, info -def run_remote_checker(spoof_proc, lat, lon, duration, ip_addr) -> bool: - checker_thread = threading.Thread(target=exec_remote_checker, - args=(lat, lon, duration, ip_addr)) - checker_thread.start() - tcnt = 0 - while True: - if not checker_thread.is_alive(): - # assume this only happens when the signal got matched - return True +stats = defaultdict(int) # type: ignore +keys = ['success', 'failed', 'ublox_fail', 'laikad_fail', 'proc_crash', 'checker_crash'] - # the spoofing process has a timeout, kill checker if reached - if not spoof_proc.is_alive(): - rc_p.kill() - # spoofing process died, assume timeout - print("Spoofing process timeout") - return False +def print_report(): + print("\nFuzzy testing report summary:") + for k in keys: + print(f" {k}: {stats[k]}") - print(f"Time elapsed: {tcnt}[s]", end = "\r") - time.sleep(1) - tcnt += 1 +def update_stats(matched, log, info): + if matched: + stats['success'] += 1 + return -def main(): - if len(sys.argv) < 2: - print(f"usage: {sys.argv[0]} [-c]") - ip_addr = sys.argv[1] + stats['failed'] += 1 + if log == "PROC CRASH": + stats['proc_crash'] += 1 + if log == "CHECKER CRASHED": + stats['checker_crash'] += 1 + if log == "TIMEOUT": + if "LAIKAD" in info: + stats['laikad_fail'] += 1 + else: # "UBLOX" in info + stats['ublox_fail'] += 1 - continuous_mode = False - if len(sys.argv) == 3 and sys.argv[2] == '-c': - print("Continuous Mode!") - continuous_mode = True +def main(ip_addr, continuous_mode, timeout, pos): rinex_file = download_rinex() - duration = 60*3 # max runtime in seconds - lat, lon = get_random_coords(47.2020, 15.7403) + lat, lon, alt = pos + if lat == 0 and lon == 0 and alt == 0: + lat, lon, alt = get_random_coords(47.2020, 15.7403) + + try: + while True: + # spoof random location + spoof_proc = run_lime_gps(rinex_file, f"{lat},{lon},{alt}", timeout) - while True: - # spoof random location - spoof_proc = run_lime_gps(rinex_file, f"{lat},{lon},100", duration) - start_time = time.monotonic() + # remote checker execs blocking + matched, log, info = run_remote_checker(lat, lon, alt, timeout, ip_addr) + update_stats(matched, log, info) + spoof_proc.terminate() + spoof_proc = None - # remote checker runs blocking - if not run_remote_checker(spoof_proc, lat, lon, duration, ip_addr): - # location could not be matched by ublox module - pass + if continuous_mode: + lat, lon, alt = get_continuous_coords(lat, lon, alt) + else: + lat, lon, alt = get_random_coords(lat, lon) + except KeyboardInterrupt: + if spoof_proc is not None: + spoof_proc.terminate() - end_time = time.monotonic() - spoof_proc.terminate() + if con is not None and not con.closed: + con.root.exposed_kill_procs() + con.close() - # -1 to count process startup - print(f"Time to get Signal: {round(end_time - start_time - 1, 4)}") + print_report() - if continuous_mode: - lat, lon = get_continuous_coords(lat, lon) - else: - lat, lon = get_random_coords(lat, lon) if __name__ == "__main__": - main() + parser = argparse.ArgumentParser(description="Fuzzy test GPS stack with random locations.") + parser.add_argument("ip_addr", type=str) + parser.add_argument("-c", "--contin", type=bool, nargs='?', default=False, help='Continous location change') + parser.add_argument("-t", "--timeout", type=int, nargs='?', default=180, help='Timeout to get location') + + # for replaying a location + parser.add_argument("lat", type=float, nargs='?', default=0) + parser.add_argument("lon", type=float, nargs='?', default=0) + parser.add_argument("alt", type=float, nargs='?', default=0) + args = parser.parse_args() + main(args.ip_addr, args.contin, args.timeout, (args.lat, args.lon, args.alt)) diff --git a/tools/gpstest/helper.py b/tools/gpstest/helper.py new file mode 100644 index 0000000000..4f62e60db0 --- /dev/null +++ b/tools/gpstest/helper.py @@ -0,0 +1,53 @@ +import random +import datetime as dt +import subprocess as sp +from typing import Tuple + +from laika.downloader import download_nav +from laika.gps_time import GPSTime +from laika.helpers import ConstellationId + + +def download_rinex(): + # TODO: check if there is a better way to get the full brdc file for LimeGPS + gps_time = GPSTime.from_datetime(dt.datetime.utcnow()) + utc_time = dt.datetime.utcnow() - dt.timedelta(1) + gps_time = GPSTime.from_datetime(dt.datetime(utc_time.year, utc_time.month, utc_time.day)) + return download_nav(gps_time, '/tmp/gpstest/', ConstellationId.GPS) + + +def exec_LimeGPS_bin(rinex_file: str, location: str, duration: int): + # this functions should never return, cause return means, timeout is + # reached or it crashed + try: + cmd = ["LimeGPS/LimeGPS", "-e", rinex_file, "-l", location] + sp.check_output(cmd, timeout=duration) + except sp.TimeoutExpired: + print("LimeGPS timeout reached!") + except Exception as e: + print(f"LimeGPS crashed: {str(e)}") + + +def get_random_coords(lat, lon) -> Tuple[float, float, int]: + # jump around the world + # max values, lat: -90 to 90, lon: -180 to 180 + + lat_add = random.random()*20 + 10 + lon_add = random.random()*20 + 20 + alt = random.randint(-10**3, 4*10**3) + + lat = ((lat + lat_add + 90) % 180) - 90 + lon = ((lon + lon_add + 180) % 360) - 180 + return round(lat, 5), round(lon, 5), alt + + +def get_continuous_coords(lat, lon, alt) -> Tuple[float, float, int]: + # continuously move around the world + lat_add = random.random()*0.01 + lon_add = random.random()*0.01 + alt_add = random.randint(-100, 100) + + lat = ((lat + lat_add + 90) % 180) - 90 + lon = ((lon + lon_add + 180) % 360) - 180 + alt += alt_add + return round(lat, 5), round(lon, 5), alt diff --git a/tools/gpstest/remote_checker.py b/tools/gpstest/remote_checker.py deleted file mode 100644 index a649a105c3..0000000000 --- a/tools/gpstest/remote_checker.py +++ /dev/null @@ -1,54 +0,0 @@ -#!/usr/bin/env python3 -import sys -import time -from typing import List - -from common.params import Params -import cereal.messaging as messaging -from selfdrive.manager.process_config import managed_processes - -DELTA = 0.001 -# assume running openpilot for now -procs: List[str] = []#"ubloxd", "pigeond"] - - -def main(): - if len(sys.argv) != 4: - print("args: ") - return - - quectel_mod = Params().get_bool("UbloxAvailable") - sol_lat = float(sys.argv[2]) - sol_lon = float(sys.argv[3]) - - for p in procs: - managed_processes[p].start() - time.sleep(0.5) # give time to startup - - socket = 'gpsLocation' if quectel_mod else 'gpsLocationExternal' - gps_sock = messaging.sub_sock(socket, timeout=0.1) - - # analyze until the location changed - while True: - events = messaging.drain_sock(gps_sock) - for e in events: - loc = e.gpsLocation if quectel_mod else e.gpsLocationExternal - lat = loc.latitude - lon = loc.longitude - - if abs(lat - sol_lat) < DELTA and abs(lon - sol_lon) < DELTA: - print("MATCH") - return - - time.sleep(0.1) - - for p in procs: - if not managed_processes[p].proc.is_alive(): - print(f"ERROR: '{p}' died") - return - - -if __name__ == "__main__": - main() - for p in procs: - managed_processes[p].stop() diff --git a/tools/gpstest/rpc_server.py b/tools/gpstest/rpc_server.py new file mode 100644 index 0000000000..b35c66d02d --- /dev/null +++ b/tools/gpstest/rpc_server.py @@ -0,0 +1,185 @@ +import os +import time +import shutil +from datetime import datetime +from collections import defaultdict + +import rpyc # pylint: disable=import-error +from rpyc.utils.server import ThreadedServer # pylint: disable=import-error + +#from common.params import Params +import cereal.messaging as messaging +from selfdrive.manager.process_config import managed_processes +from laika.lib.coordinates import ecef2geodetic + +DELTA = 0.001 +ALT_DELTA = 30 +MATCH_NUM = 10 +REPORT_STATS = 10 + +EPHEM_CACHE = "/data/params/d/LaikadEphemeris" +DOWNLOAD_CACHE = "/tmp/comma_download_cache" + +SERVER_LOG_FILE = "/tmp/fuzzy_server.log" +server_log = open(SERVER_LOG_FILE, "w+") + +def slog(msg): + server_log.write(f"{datetime.now().strftime('%H:%M:%S.%f')} | {msg}\n") + server_log.flush() + +def handle_laikad(msg): + if not hasattr(msg, 'correctedMeasurements'): + return None + + num_corr = len(msg.correctedMeasurements) + pos_ecef = msg.positionECEF.value + pos_geo = [] + if len(pos_ecef) > 0: + pos_geo = ecef2geodetic(pos_ecef) + + pos_std = msg.positionECEF.std + pos_valid = msg.positionECEF.valid + + slog(f"{num_corr} {pos_geo} {pos_ecef} {pos_std} {pos_valid}") + return pos_geo, (num_corr, pos_geo, list(pos_ecef), list(msg.positionECEF.std)) + +hw_msgs = 0 +ephem_msgs: dict = defaultdict(int) +def handle_ublox(msg): + global hw_msgs + + d = msg.to_dict() + + if 'hwStatus2' in d: + hw_msgs += 1 + + if 'ephemeris' in d: + ephem_msgs[msg.ephemeris.svId] += 1 + + num_meas = None + if 'measurementReport' in d: + num_meas = msg.measurementReport.numMeas + + return [hw_msgs, ephem_msgs, num_meas] + + +def start_procs(procs): + for p in procs: + managed_processes[p].start() + time.sleep(1) + +def kill_procs(procs, no_retry=False): + for p in procs: + managed_processes[p].stop() + time.sleep(1) + + if not no_retry: + for p in procs: + mp = managed_processes[p].proc + if mp is not None and mp.is_alive(): + managed_processes[p].stop() + time.sleep(3) + +def check_alive_procs(procs): + for p in procs: + mp = managed_processes[p].proc + if mp is None or not mp.is_alive(): + return False, p + return True, None + + +class RemoteCheckerService(rpyc.Service): + def on_connect(self, conn): + pass + + def on_disconnect(self, conn): + #kill_procs(self.procs, no_retry=False) + # this execution is delayed, it will kill the next run of laikad + # TODO: add polling to wait for everything is killed + pass + + def run_checker(self, slat, slon, salt, sockets, procs, timeout): + global hw_msgs, ephem_msgs + hw_msgs = 0 + ephem_msgs = defaultdict(int) + + slog(f"Run test: {slat} {slon} {salt}") + + # quectel_mod = Params().get_bool("UbloxAvailable") + + match_cnt = 0 + msg_cnt = 0 + stats_laikad = [] + stats_ublox = [] + + self.procs = procs + start_procs(procs) + sm = messaging.SubMaster(sockets) + + start_time = time.monotonic() + while True: + sm.update() + + if sm.updated['ubloxGnss']: + stats_ublox.append(handle_ublox(sm['ubloxGnss'])) + + if sm.updated['gnssMeasurements']: + pos_geo, stats = handle_laikad(sm['gnssMeasurements']) + if pos_geo is None or len(pos_geo) == 0: + continue + + match = all(abs(g-s) < DELTA for g,s in zip(pos_geo[:2], [slat, slon])) + match &= abs(pos_geo[2] - salt) < ALT_DELTA + if match: + match_cnt += 1 + if match_cnt >= MATCH_NUM: + return True, "MATCH", f"After: {round(time.monotonic() - start_time, 4)}" + + # keep some stats for error reporting + stats_laikad.append(stats) + + if (msg_cnt % 10) == 0: + a, p = check_alive_procs(procs) + if not a: + return False, "PROC CRASH", f"{p}" + msg_cnt += 1 + + if (time.monotonic() - start_time) > timeout: + h = f"LAIKAD: {stats_laikad[-REPORT_STATS:]}" + if len(h) == 0: + h = f"UBLOX: {stats_ublox[-REPORT_STATS:]}" + return False, "TIMEOUT", h + + + def exposed_run_checker(self, slat, slon, salt, timeout=180, use_laikad=True): + try: + procs = [] + sockets = [] + + if use_laikad: + procs.append("laikad") # pigeond, ubloxd # might wanna keep them running + sockets += ['ubloxGnss', 'gnssMeasurements'] + + if os.path.exists(EPHEM_CACHE): + os.remove(EPHEM_CACHE) + shutil.rmtree(DOWNLOAD_CACHE, ignore_errors=True) + + ret = self.run_checker(slat, slon, salt, sockets, procs, timeout) + kill_procs(procs) + return ret + + except Exception as e: + # always make sure processes get killed + kill_procs(procs) + return False, "CHECKER CRASHED", f"{str(e)}" + + + def exposed_kill_procs(self): + kill_procs(self.procs, no_retry=True) + + +if __name__ == "__main__": + print(f"Sever Log written to: {SERVER_LOG_FILE}") + t = ThreadedServer(RemoteCheckerService, port=18861) + t.start() + diff --git a/tools/gpstest/simulate_gps_signal.py b/tools/gpstest/simulate_gps_signal.py index a6aca1c404..da0f64eaca 100755 --- a/tools/gpstest/simulate_gps_signal.py +++ b/tools/gpstest/simulate_gps_signal.py @@ -16,7 +16,7 @@ cache_dir = '/tmp/gpstest/' def download_rinex(): # TODO: check if there is a better way to get the full brdc file for LimeGPS gps_time = GPSTime.from_datetime(dt.datetime.utcnow()) - utc_time = dt.datetime.utcnow() - dt.timedelta(1) + utc_time = dt.datetime.utcnow()# - dt.timedelta(1) gps_time = GPSTime.from_datetime(dt.datetime(utc_time.year, utc_time.month, utc_time.day)) return download_nav(gps_time, cache_dir, ConstellationId.GPS) @@ -36,11 +36,15 @@ def get_random_coords(lat, lon) -> Tuple[int, int]: # jump around the world return get_coords(lat, lon, 20, 20, 10, 20) -def run_limeSDR_loop(lat, lon, contin_sim, rinex_file, timeout): +def run_limeSDR_loop(lat, lon, alt, contin_sim, rinex_file, timeout): while True: try: - print(f"starting LimeGPS, Location: {lat},{lon}") - cmd = ["LimeGPS/LimeGPS", "-e", rinex_file, "-l", f"{lat},{lon},100"] + # TODO: add starttime setting and altitude + # -t 2023/01/15,00:00:00 -T 2023/01/15,00:00:00 + # this needs to match the date of the navigation file + print(f"starting LimeGPS, Location: {lat} {lon} {alt}") + cmd = ["LimeGPS/LimeGPS", "-e", rinex_file, "-l", f"{lat},{lon},{alt}"] + print(f"CMD: {cmd}") sp.check_output(cmd, stderr=sp.PIPE, timeout=timeout) except KeyboardInterrupt: print("stopping LimeGPS") @@ -71,7 +75,7 @@ def run_hackRF_loop(lat, lon, rinex_file, timeout): try: print(f"starting gps-sdr-sim, Location: {lat},{lon}") # create 30second file and replay with hackrf endless - cmd = ["gps-sdr-sim/gps-sdr-sim", "-e", rinex_file, "-l", f"{lat},{lon},100", "-d", "30"] + cmd = ["gps-sdr-sim/gps-sdr-sim", "-e", rinex_file, "-l", f"{lat},{lon},-200", "-d", "30"] sp.check_output(cmd, stderr=sp.PIPE, timeout=timeout) # created in current working directory except Exception: @@ -90,7 +94,7 @@ def run_hackRF_loop(lat, lon, rinex_file, timeout): print(f"hackrf_transfer crashed:{str(e)}") -def main(lat, lon, jump_sim, contin_sim, hackrf_mode): +def main(lat, lon, alt, jump_sim, contin_sim, hackrf_mode): if hackrf_mode: if not os.path.exists('hackrf'): @@ -130,17 +134,18 @@ def main(lat, lon, jump_sim, contin_sim, hackrf_mode): if jump_sim: timeout = 30 - if not hackrf_mode: - run_limeSDR_loop(lat, lon, contin_sim, rinex_file, timeout) - else: + if hackrf_mode: run_hackRF_loop(lat, lon, rinex_file, timeout) + else: + run_limeSDR_loop(lat, lon, alt, contin_sim, rinex_file, timeout) if __name__ == "__main__": parser = argparse.ArgumentParser(description="Simulate static [or random jumping] GPS signal.") parser.add_argument("lat", type=float, nargs='?', default=0) parser.add_argument("lon", type=float, nargs='?', default=0) + parser.add_argument("alt", type=float, nargs='?', default=0) parser.add_argument("--jump", action="store_true", help="signal that jumps around the world") parser.add_argument("--contin", action="store_true", help="continuously/slowly moving around the world") parser.add_argument("--hackrf", action="store_true", help="hackrf mode (DEFAULT: LimeSDR)") args = parser.parse_args() - main(args.lat, args.lon, args.jump, args.contin, args.hackrf) + main(args.lat, args.lon, args.alt, args.jump, args.contin, args.hackrf) diff --git a/tools/gpstest/test_laikad.py b/tools/gpstest/test_laikad.py new file mode 100644 index 0000000000..613ac991b1 --- /dev/null +++ b/tools/gpstest/test_laikad.py @@ -0,0 +1,105 @@ +#!/usr/bin/env python3 +import os +import time +import unittest + +import cereal.messaging as messaging +import selfdrive.sensord.pigeond as pd + +from common.params import Params +from system.hardware import TICI +from selfdrive.manager.process_config import managed_processes +from selfdrive.test.helpers import with_processes + + +def wait_for_location(sm, timeout, con=10): + cons_meas = 0 + start_time = time.monotonic() + while (time.monotonic() - start_time) < timeout: + sm.update() + if not sm.updated["gnssMeasurements"]: + continue + + msg = sm["gnssMeasurements"] + cons_meas = (cons_meas + 1) if 'positionECEF' in msg.to_dict() else 0 + if cons_meas >= con: + return True + return False + + +class TestLaikad(unittest.TestCase): + @classmethod + def setUpClass(self): + if not TICI: + raise unittest.SkipTest + + ublox_available = Params().get_bool("UbloxAvailable") + if not ublox_available: + raise unittest.SkipTest + + def setUp(self): + # ensure laikad cold start + Params().remove("LaikadEphemeris") + os.environ["LAIKAD_NO_INTERNET"] = "1" + managed_processes['laikad'].start() + + def tearDown(self): + managed_processes['laikad'].stop() + + + @with_processes(['pigeond', 'ubloxd']) + def test_laikad_cold_start(self): + time.sleep(5) + + start_time = time.monotonic() + sm = messaging.SubMaster(["gnssMeasurements"]) + + success = wait_for_location(sm, 60*2, con=10) + duration = time.monotonic() - start_time + + assert success, "Waiting for location timed out (2min)!" + assert duration < 60, f"Received Location {duration}!" + + + @with_processes(['ubloxd']) + def test_laikad_ublox_reset_start(self): + time.sleep(2) + + pigeon, pm = pd.create_pigeon() + pd.init_baudrate(pigeon) + assert pigeon.reset_device(), "Could not reset device!" + + laikad_sock = messaging.sub_sock("gnssMeasurements", timeout=0.1) + ublox_gnss_sock = messaging.sub_sock("ubloxGnss", timeout=0.1) + + pd.init_baudrate(pigeon) + pd.initialize_pigeon(pigeon) + pd.run_receiving(pigeon, pm, 180) + + ublox_msgs = messaging.drain_sock(ublox_gnss_sock) + laikad_msgs = messaging.drain_sock(laikad_sock) + + gps_ephem_cnt = 0 + glonass_ephem_cnt = 0 + for um in ublox_msgs: + if um.ubloxGnss.which() == 'ephemeris': + gps_ephem_cnt += 1 + elif um.ubloxGnss.which() == 'glonassEphemeris': + glonass_ephem_cnt += 1 + + assert gps_ephem_cnt > 0, "NO gps ephemeris collected!" + assert glonass_ephem_cnt > 0, "NO glonass ephemeris collected!" + + pos_meas = 0 + duration = -1 + for lm in laikad_msgs: + pos_meas = (pos_meas + 1) if 'positionECEF' in lm.gnssMeasurements.to_dict() else 0 + if pos_meas > 5: + duration = (lm.logMonoTime - laikad_msgs[0].logMonoTime)*1e-9 + break + + assert pos_meas > 5, "NOT enough positions at end of read!" + assert duration < 120, "Laikad took too long to get a Position!" + +if __name__ == "__main__": + unittest.main() diff --git a/tools/replay/replay.cc b/tools/replay/replay.cc index 998c93b938..178b116a87 100644 --- a/tools/replay/replay.cc +++ b/tools/replay/replay.cc @@ -25,6 +25,13 @@ Replay::Replay(QString route, QStringList allow, QStringList block, SubMaster *s s.push_back(it.name); } } + + if (!allow_list.empty()) { + // the following events are needed for replay to work properly. + allow_list.insert(cereal::Event::Which::INIT_DATA); + allow_list.insert(cereal::Event::Which::CAR_PARAMS); + } + qDebug() << "services " << s; qDebug() << "loading route " << route; @@ -107,9 +114,9 @@ void Replay::seekTo(double seconds, bool relative) { rInfo("seeking to %d s, segment %d", (int)seconds, seg); current_segment_ = seg; cur_mono_time_ = route_start_ts_ + seconds * 1e9; + emit seekedTo(seconds); return isSegmentMerged(seg); }); - emit seekedTo(seconds); queueSegment(); } diff --git a/tools/ubuntu_setup.sh b/tools/ubuntu_setup.sh index 09296ef94d..71bad2e8a2 100755 --- a/tools/ubuntu_setup.sh +++ b/tools/ubuntu_setup.sh @@ -86,6 +86,7 @@ function install_ubuntu_lts_latest_requirements() { install_ubuntu_common_requirements $SUDO apt-get install -y --no-install-recommends \ + g++-12 \ qtbase5-dev \ qtchooser \ qt5-qmake \