sadmen
/
openpilot_comma
mirror of https://github.com/commaai/openpilot.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'master' into mqb-long

Browse Source 
# Conflicts:
#	opendbc
#	panda
#	selfdrive/car/volkswagen/carcontroller.py
#	selfdrive/car/volkswagen/carstate.py
pull/22963/head
Jason Young 3 years ago
parent 547f1cd435 12f74ade98
commit bd4f1a682d
189 changed files with 3811 additions and 2920 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 6
      .gitattributes
  2. 27
      .github/workflows/selfdrive_tests.yaml
  3. 16
      .pre-commit-config.yaml
  4. 5
      .pylintrc
  5. 4
      RELEASES.md
  6. 2
      cereal
  7. 1
      common/params_pyx.pyx
  8. 8
      common/tests/test_params.py
  9. 187
      docs/CARS.md
  10. 2
      launch_env.sh
  11. 5
      pyextra/acados_template/acados_ocp_solver_pyx.pyx
  12. 42
      release/files_common
  13. 47
      selfdrive/athena/athenad.py
  14. 9
      selfdrive/athena/registration.py
  15. 25
      selfdrive/athena/tests/test_athenad.py
  16. 2
      selfdrive/boardd/SConscript
  17. 128
      selfdrive/boardd/boardd.cc
  18. 6
      selfdrive/boardd/boardd.h
  19. 22
      selfdrive/boardd/main.cc
  20. 14
      selfdrive/boardd/panda.cc
  21. 24
      selfdrive/boardd/panda.h
  22. 29
      selfdrive/camerad/cameras/camera_common.cc
  23. 6
      selfdrive/camerad/cameras/camera_common.h
  24. 166
      selfdrive/camerad/cameras/camera_qcom2.cc
  25. 54
      selfdrive/camerad/main.cc
  26. 5
      selfdrive/camerad/test/ae_gray_test.cc
  27. 2
      selfdrive/car/car_helpers.py
  28. 68
      selfdrive/car/chrysler/carstate.py
  29. 3
      selfdrive/car/chrysler/interface.py
  30. 31
      selfdrive/car/chrysler/radar_interface.py
  31. 2
      selfdrive/car/chrysler/values.py
  32. 28
      selfdrive/car/ford/carstate.py
  33. 2
      selfdrive/car/ford/interface.py
  34. 6
      selfdrive/car/ford/radar_interface.py
  35. 2
      selfdrive/car/ford/values.py
  36. 6
      selfdrive/car/gm/carcontroller.py
  37. 64
      selfdrive/car/gm/carstate.py
  38. 4
      selfdrive/car/gm/interface.py
  39. 19
      selfdrive/car/gm/radar_interface.py
  40. 16
      selfdrive/car/gm/values.py
  41. 4
      selfdrive/car/honda/carcontroller.py
  42. 160
      selfdrive/car/honda/carstate.py
  43. 13
      selfdrive/car/honda/interface.py
  44. 8
      selfdrive/car/honda/radar_interface.py
  45. 84
      selfdrive/car/honda/values.py
  46. 6
      selfdrive/car/hyundai/carcontroller.py
  47. 205
      selfdrive/car/hyundai/carstate.py
  48. 2
      selfdrive/car/hyundai/interface.py
  49. 10
      selfdrive/car/hyundai/radar_interface.py
  50. 8
      selfdrive/car/hyundai/values.py
  51. 8
      selfdrive/car/interfaces.py
  52. 2
      selfdrive/car/mazda/carcontroller.py
  53. 125
      selfdrive/car/mazda/carstate.py
  54. 12
      selfdrive/car/mazda/interface.py
  55. 31
      selfdrive/car/mazda/values.py
  56. 8
      selfdrive/car/nissan/carcontroller.py
  57. 284
      selfdrive/car/nissan/carstate.py
  58. 4
      selfdrive/car/nissan/interface.py
  59. 2
      selfdrive/car/nissan/values.py
  60. 4
      selfdrive/car/subaru/carcontroller.py
  61. 179
      selfdrive/car/subaru/carstate.py
  62. 2
      selfdrive/car/subaru/interface.py
  63. 10
      selfdrive/car/subaru/values.py
  64. 6
      selfdrive/car/tesla/carcontroller.py
  65. 116
      selfdrive/car/tesla/carstate.py
  66. 3
      selfdrive/car/tesla/interface.py
  67. 26
      selfdrive/car/tesla/radar_interface.py
  68. 2
      selfdrive/car/tesla/values.py
  69. 6
      selfdrive/car/toyota/carcontroller.py
  70. 118
      selfdrive/car/toyota/carstate.py
  71. 54
      selfdrive/car/toyota/interface.py
  72. 8
      selfdrive/car/toyota/radar_interface.py
  73. 29
      selfdrive/car/toyota/toyotacan.py
  74. 169
      selfdrive/car/toyota/values.py
  75. 4
      selfdrive/car/volkswagen/carcontroller.py
  76. 144
      selfdrive/car/volkswagen/carstate.py
  77. 10
      selfdrive/car/volkswagen/interface.py
  78. 9
      selfdrive/car/volkswagen/values.py
  79. 2
      selfdrive/common/SConscript
  80. 14
      selfdrive/common/params.cc
  81. 7
      selfdrive/common/params.h
  82. 43
      selfdrive/common/statlog.cc
  83. 10
      selfdrive/common/statlog.h
  84. 114
      selfdrive/common/swaglog.cc
  85. 4
      selfdrive/common/swaglog.h
  86. 1
      selfdrive/common/tests/.gitignore
  87. 94
      selfdrive/common/tests/test_swaglog.cc
  88. 29
      selfdrive/common/util.h
  89. 90
      selfdrive/controls/controlsd.py
  90. 16
      selfdrive/controls/lib/alertmanager.py
  91. 2
      selfdrive/controls/lib/alerts_offroad.json
  92. 113
      selfdrive/controls/lib/desire_helper.py
  93. 17
      selfdrive/controls/lib/events.py
  94. 28
      selfdrive/controls/lib/latcontrol.py
  95. 15
      selfdrive/controls/lib/latcontrol_angle.py
  96. 61
      selfdrive/controls/lib/latcontrol_indi.py
  97. 30
      selfdrive/controls/lib/latcontrol_lqr.py
  98. 15
      selfdrive/controls/lib/latcontrol_pid.py
  99. 39
      selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py
  100. 146
      selfdrive/controls/lib/lateral_planner.py
  101. Some files were not shown because too many files have changed in this diff Show More

6
.gitattributes vendored
Unescape View File

@ -1,8 +1,2 @@
*.keras filter=lfs diff=lfs merge=lfs -text
*.dlc filter=lfs diff=lfs merge=lfs -text
*.onnx filter=lfs diff=lfs merge=lfs -text
*.pb filter=lfs diff=lfs merge=lfs -text
*.bin filter=lfs diff=lfs merge=lfs -text
*.jpg filter=lfs diff=lfs merge=lfs -text
*.ipynb filter=nbstripout -diff
external/ffmpeg/bin/ffmpeg_cuda filter=lfs diff=lfs merge=lfs -text

27
.github/workflows/selfdrive_tests.yaml
Unescape View File

@ -260,12 +260,14 @@ jobs:
$UNIT_TEST tools/lib/tests && \
./selfdrive/boardd/tests/test_boardd_usbprotocol && \
./selfdrive/common/tests/test_util && \
./selfdrive/common/tests/test_swaglog && \
./selfdrive/loggerd/tests/test_logger &&\
./selfdrive/proclogd/tests/test_proclog && \
./selfdrive/ui/replay/tests/test_replay && \
./selfdrive/camerad/test/ae_gray_test"
- name: Upload coverage to Codecov
run: bash <(curl -s https://codecov.io/bash) -v -F unit_tests
./selfdrive/camerad/test/ae_gray_test && \
coverage xml"
- name: "Upload coverage to Codecov"
uses: codecov/codecov-action@v2
process_replay:
name: process replay
@ -298,9 +300,10 @@ jobs:
- name: Run replay
run: |
${{ env.RUN }} "scons -j$(nproc) && \
FILEREADER_CACHE=1 CI=1 coverage run selfdrive/test/process_replay/test_processes.py"
- name: Upload coverage to Codecov
run: bash <(curl -s https://codecov.io/bash) -v -F process_replay
FILEREADER_CACHE=1 CI=1 coverage run selfdrive/test/process_replay/test_processes.py && \
coverage xml"
- name: "Upload coverage to Codecov"
uses: codecov/codecov-action@v2
- name: Print diff
if: always()
run: cat selfdrive/test/process_replay/diff.txt
@ -353,7 +356,10 @@ jobs:
${{ env.RUN }} "mkdir -p selfdrive/test/out && \
scons -j$(nproc) && \
cd selfdrive/test/longitudinal_maneuvers && \
./test_longitudinal.py"
coverage run ./test_longitudinal.py && \
coverage xml"
- name: "Upload coverage to Codecov"
uses: codecov/codecov-action@v2
- uses: actions/upload-artifact@v2
if: always()
continue-on-error: true
@ -396,13 +402,14 @@ jobs:
- name: Test car models
run: |
${{ env.RUN }} "scons -j$(nproc) --test && \
FILEREADER_CACHE=1 pytest selfdrive/test/test_models.py && \
FILEREADER_CACHE=1 coverage run -m pytest selfdrive/test/test_models.py && \
coverage xml && \
chmod -R 777 /tmp/comma_download_cache"
env:
NUM_JOBS: 4
JOB_ID: ${{ matrix.job }}
- name: Upload coverage to Codecov
run: bash <(curl -s https://codecov.io/bash) -v -F test_car_models
- name: "Upload coverage to Codecov"
uses: codecov/codecov-action@v2
docs:
name: build docs

16
.pre-commit-config.yaml
Unescape View File

@ -1,6 +1,10 @@
repos:
- repo: meta
hooks:
- id: check-hooks-apply
- id: check-useless-excludes
- repo: https://github.com/pre-commit/pre-commit-hooks
rev: v4.0.1
rev: v4.1.0
hooks:
- id: check-ast
exclude: '^(pyextra)/'
@ -9,8 +13,10 @@ repos:
- id: check-yaml
- id: check-merge-conflict
- id: check-symlinks
- id: check-added-large-files
args: ['--maxkb=100']
- repo: https://github.com/pre-commit/mirrors-mypy
rev: v0.910-1
rev: v0.931
hooks:
- id: mypy
exclude: '^(pyextra/)|(cereal/)|(rednose/)|(panda/)|(laika/)|(opendbc/)|(laika_repo/)|(rednose_repo/)/'
@ -21,9 +27,13 @@ repos:
hooks:
- id: flake8
exclude: '^(pyextra/)|(cereal/)|(rednose/)|(panda/)|(laika/)|(opendbc/)|(laika_repo/)|(rednose_repo/)|(selfdrive/debug/)/'
additional_dependencies: ['flake8-no-implicit-concat']
args:
- --indent-size=2
- --enable-extensions=NIC
- --select=F,E112,E113,E304,E502,E701,E702,E703,E71,E72,E731,W191,W6
- --statistics
- -j4
- repo: local
hooks:
- id: pylint
@ -31,7 +41,7 @@ repos:
entry: pylint
language: system
types: [python]
exclude: '^(pyextra/)|(cereal/)|(rednose/)|(panda/)|(laika/)|(laika_repo/)|(rednose_repo/)/'
exclude: '^(pyextra/)|(cereal/)|(rednose/)|(panda/)|(laika/)|(laika_repo/)|(rednose_repo/)'
- repo: local
hooks:
- id: cppcheck

5
.pylintrc
Unescape View File

@ -463,6 +463,11 @@ known-standard-library=
# Force import order to recognize a module as part of a third party library.
known-third-party=enchant
[STRING]
# This flag controls whether the implicit-str-concat should generate a warning
# on implicit string concatenation in sequences defined over several lines.
check-str-concat-over-line-jumps=yes
[EXCEPTIONS]

4
RELEASES.md
Unescape View File

@ -1,10 +1,14 @@
Version 0.8.13 (2022-XX-XX)
========================
* Improved driver monitoring
* Roll compensation
* Improved camera focus on the comma two
* Subaru ECU firmware fingerprinting thanks to martinl!
* Hyundai Santa Fe Plug-in Hybrid 2022 support thanks to sunnyhaibin!
* Mazda CX-5 2022 support thanks to Jafaral!
* Subaru Impreza 2020 support thanks to martinl!
* Toyota Avalon 2022 support thanks to sshane!
* Toyota Prius v 2017 support thanks to CT921!
Version 0.8.12 (2021-12-15)
========================

2
cereal

@ -1 +1 @@
Subproject commit 42542ee96ca00744e6117d57533defe6f01ba14d
Subproject commit 03860ae0b2b8128cae7768e4301d889e627c9275

1
common/params_pyx.pyx
Unescape View File

@ -8,7 +8,6 @@ cdef extern from "selfdrive/common/params.h":
cpdef enum ParamKeyType:
PERSISTENT
CLEAR_ON_MANAGER_START
CLEAR_ON_PANDA_DISCONNECT
CLEAR_ON_IGNITION_ON
CLEAR_ON_IGNITION_OFF
ALL

8
common/tests/test_params.py
Unescape View File

@ -24,14 +24,6 @@ class TestParams(unittest.TestCase):
self.params.put("CarParams", st)
assert self.params.get("CarParams") == st
def test_params_get_cleared_panda_disconnect(self):
self.params.put("CarParams", "test")
self.params.put("DongleId", "cb38263377b873ee")
assert self.params.get("CarParams") == b"test"
self.params.clear_all(ParamKeyType.CLEAR_ON_PANDA_DISCONNECT)
assert self.params.get("CarParams") is None
assert self.params.get("DongleId") is not None
def test_params_get_cleared_manager_start(self):
self.params.put("CarParams", "test")
self.params.put("DongleId", "cb38263377b873ee")

187
docs/CARS.md
Unescape View File

@ -43,8 +43,9 @@
| Lexus | RX Hybrid 2016-19 | All | Stock<sup>3</sup>| 0mph | 0mph |
| Lexus | RX Hybrid 2020-21 | All | openpilot | 0mph | 0mph |
| Lexus | UX Hybrid 2019-21 | All | openpilot | 0mph | 0mph |
| Toyota | Alphard 2020 | All | openpilot | 0mph | 0mph |
| Toyota | Alphard 2019-20 | All | openpilot | 0mph | 0mph |
| Toyota | Avalon 2016-21 | TSS-P | Stock<sup>3</sup>| 20mph<sup>1</sup> | 0mph |
| Toyota | Avalon 2022 | All | openpilot | 0mph | 0mph |
| Toyota | Avalon Hybrid 2019-21 | TSS-P | Stock<sup>3</sup>| 20mph<sup>1</sup> | 0mph |
| Toyota | Camry 2018-20 | All | Stock | 0mph<sup>4</sup> | 0mph |
| Toyota | Camry 2021-22 | All | openpilot | 0mph<sup>4</sup> | 0mph |
@ -63,6 +64,7 @@
| Toyota | Mirai 2021 | All | openpilot | 0mph | 0mph |
| Toyota | Prius 2016-20 | TSS-P | Stock<sup>3</sup>| 0mph | 0mph |
| Toyota | Prius 2021-22 | All | openpilot | 0mph | 0mph |
| Toyota | Prius v 2017 | TSS-P | Stock<sup>3</sup>| 20mph<sup>1</sup> | 0mph |
| Toyota | Prius Prime 2017-20 | All | Stock<sup>3</sup>| 0mph | 0mph |
| Toyota | Prius Prime 2021-22 | All | openpilot | 0mph | 0mph |
| Toyota | Rav4 2016-18 | TSS-P | Stock<sup>3</sup>| 20mph<sup>1</sup> | 0mph |
@ -78,99 +80,96 @@
## Community Maintained Cars and Features
| Make | Model (US Market Reference) | Supported Package | ACC | No ACC accel below | No ALC below |
| ----------| ------------------------------| ------------------| -----------------| -------------------| -------------|
| Audi | A3 2014-19 | ACC + Lane Assist | Stock | 0mph | 0mph |
| Audi | A3 Sportback e-tron 2017-18 | ACC + Lane Assist | Stock | 0mph | 0mph |
| Audi | Q2 2018 | ACC + Lane Assist | Stock | 0mph | 0mph |
| Audi | Q3 2020-21 | ACC + Lane Assist | Stock | 0mph | 0mph |
| Audi | S3 2015 | ACC + Lane Assist | Stock | 0mph | 0mph |
| Buick | Regal 2018<sup>1</sup> | Adaptive Cruise | openpilot | 0mph | 7mph |
| Cadillac | ATS 2018<sup>1</sup> | Adaptive Cruise | openpilot | 0mph | 7mph |
| Cadillac | Escalade ESV 2016<sup>1</sup> | ACC + LKAS | openpilot | 0mph | 7mph |
| Chevrolet | Malibu 2017<sup>1</sup> | Adaptive Cruise | openpilot | 0mph | 7mph |
| Chevrolet | Volt 2017-18<sup>1</sup> | Adaptive Cruise | openpilot | 0mph | 7mph |
| Chrysler | Pacifica 2017-18 | Adaptive Cruise | Stock | 0mph | 9mph |
| Chrysler | Pacifica 2020 | Adaptive Cruise | Stock | 0mph | 39mph |
| Chrysler | Pacifica Hybrid 2017-18 | Adaptive Cruise | Stock | 0mph | 9mph |
| Chrysler | Pacifica Hybrid 2019-21 | Adaptive Cruise | Stock | 0mph | 39mph |
| Genesis | G70 2018 | All | Stock | 0mph | 0mph |
| Genesis | G70 2020 | All | Stock | 0mph | 0mph |
| Genesis | G80 2018 | All | Stock | 0mph | 0mph |
| Genesis | G90 2018 | All | Stock | 0mph | 0mph |
| GMC | Acadia 2018<sup>1</sup> | Adaptive Cruise | openpilot | 0mph | 7mph |
| Holden | Astra 2017<sup>1</sup> | Adaptive Cruise | openpilot | 0mph | 7mph |
| Hyundai | Elantra 2017-19 | SCC + LKAS | Stock | 19mph | 34mph |
| Hyundai | Elantra 2021 | SCC + LKAS | Stock | 0mph | 0mph |
| Hyundai | Elantra Hybrid 2021 | SCC + LKAS | Stock | 0mph | 0mph |
| Hyundai | Genesis 2015-16 | SCC + LKAS | Stock | 19mph | 37mph |
| Hyundai | Ioniq Electric 2019 | SCC + LKAS | Stock | 0mph | 32mph |
| Hyundai | Ioniq Electric 2020 | SCC + LKAS | Stock | 0mph | 0mph |
| Hyundai | Ioniq Hybrid 2017-19 | SCC + LKAS | Stock | 0mph | 32mph |
| Hyundai | Ioniq Hybrid 2020-22 | SCC + LFA | Stock | 0mph | 0mph |
| Hyundai | Ioniq PHEV 2020-21 | SCC + LKAS | Stock | 0mph | 0mph |
| Hyundai | Kona 2020 | SCC + LKAS | Stock | 0mph | 0mph |
| Hyundai | Kona EV 2018-19 | SCC + LKAS | Stock | 0mph | 0mph |
| Hyundai | Kona Hybrid 2020 | SCC + LKAS | Stock | 0mph | 0mph |
| Hyundai | Santa Fe 2019-20 | All | Stock | 0mph | 0mph |
| Hyundai | Santa Fe 2021-22 | All | Stock | 0mph | 0mph |
| Hyundai | Santa Fe Hybrid 2022 | All | Stock | 0mph | 0mph |
| Hyundai | Santa Fe Plug-in Hybrid 2022 | All | Stock | 0mph | 0mph |
| Hyundai | Sonata 2018-2019 | SCC + LKAS | Stock | 0mph | 0mph |
| Hyundai | Sonata Hybrid 2021-22 | All | Stock | 0mph | 0mph |
| Hyundai | Veloster 2019-20 | SCC + LKAS | Stock | 5mph | 0mph |
| Jeep | Grand Cherokee 2016-18 | Adaptive Cruise | Stock | 0mph | 9mph |
| Jeep | Grand Cherokee 2019-20 | Adaptive Cruise | Stock | 0mph | 39mph |
| Kia | Ceed 2019 | SCC + LKAS | Stock | 0mph | 0mph |
| Kia | Forte 2018-21 | SCC + LKAS | Stock | 0mph | 0mph |
| Kia | K5 2021-22 | SCC + LFA | Stock | 0mph | 0mph |
| Kia | Niro EV 2019-22 | All | Stock | 0mph | 0mph |
| Kia | Niro Hybrid 2021 | SCC + LKAS | Stock | 0mph | 0mph |
| Kia | Niro PHEV 2019 | SCC + LKAS | Stock | 10mph | 32mph |
| Kia | Optima 2017 | SCC + LKAS | Stock | 0mph | 32mph |
| Kia | Optima 2019 | SCC + LKAS | Stock | 0mph | 0mph |
| Kia | Seltos 2021 | SCC + LKAS | Stock | 0mph | 0mph |
| Kia | Sorento 2018-19 | SCC + LKAS | Stock | 0mph | 0mph |
| Kia | Stinger 2018 | SCC + LKAS | Stock | 0mph | 0mph |
| Kia | Telluride 2020 | SCC + LKAS | Stock | 0mph | 0mph |
| Mazda | CX-9 2021 | All | Stock | 0mph | 28mph |
| Nissan | Altima 2019-20 | ProPILOT | Stock | 0mph | 0mph |
| Nissan | Leaf 2018-22 | ProPILOT | Stock | 0mph | 0mph |
| Nissan | Rogue 2018-20 | ProPILOT | Stock | 0mph | 0mph |
| Nissan | X-Trail 2017 | ProPILOT | Stock | 0mph | 0mph |
| SEAT | Ateca 2018 | Driver Assistance | Stock | 0mph | 0mph |
| SEAT | Leon 2014-2020 | Driver Assistance | Stock | 0mph | 0mph |
| Škoda | Kamiq 2021<sup>2</sup> | Driver Assistance | Stock | 0mph | 0mph |
| Škoda | Karoq 2019 | Driver Assistance | Stock | 0mph | 0mph |
| Škoda | Kodiaq 2018-19 | Driver Assistance | Stock | 0mph | 0mph |
| Škoda | Octavia 2015, 2018-19 | Driver Assistance | Stock | 0mph | 0mph |
| Škoda | Octavia RS 2016 | Driver Assistance | Stock | 0mph | 0mph |
| Škoda | Scala 2020 | Driver Assistance | Stock | 0mph | 0mph |
| Škoda | Superb 2015-18 | Driver Assistance | Stock | 0mph | 0mph |
| Subaru | Ascent 2019 | EyeSight | Stock | 0mph | 0mph |
| Subaru | Crosstrek 2018-20 | EyeSight | Stock | 0mph | 0mph |
| Subaru | Forester 2019-21 | EyeSight | Stock | 0mph | 0mph |
| Subaru | Impreza 2017-19 | EyeSight | Stock | 0mph | 0mph |
| Volkswagen| Arteon 2018, 2021<sup>4</sup> | Driver Assistance | Stock | 0mph | 0mph |
| Volkswagen| Atlas 2018-19 | Driver Assistance | Stock | 0mph | 0mph |
| Volkswagen| California 2021<sup>4</sup> | Driver Assistance | Stock | 0mph | 32mph |
| Volkswagen| e-Golf 2014, 2019-20 | Driver Assistance | Stock | 0mph | 0mph |
| Volkswagen| Golf 2015-20 | Driver Assistance | Stock | 0mph | 0mph |
| Volkswagen| Golf Alltrack 2017-18 | Driver Assistance | Stock | 0mph | 0mph |
| Volkswagen| Golf GTE 2016 | Driver Assistance | Stock | 0mph | 0mph |
| Volkswagen| Golf GTI 2018-20 | Driver Assistance | Stock | 0mph | 0mph |
| Volkswagen| Golf R 2016-19 | Driver Assistance | Stock | 0mph | 0mph |
| Volkswagen| Golf SportsVan 2016 | Driver Assistance | Stock | 0mph | 0mph |
| Volkswagen| Golf SportWagen 2015 | Driver Assistance | Stock | 0mph | 0mph |
| Volkswagen| Jetta 2018-20 | Driver Assistance | Stock | 0mph | 0mph |
| Volkswagen| Jetta GLI 2021 | Driver Assistance | Stock | 0mph | 0mph |
| Volkswagen| Passat 2016-18<sup>3</sup> | Driver Assistance | Stock | 0mph | 0mph |
| Volkswagen| Polo 2020 | Driver Assistance | Stock | 0mph | 0mph |
| Volkswagen| T-Cross 2021<sup>4</sup> | Driver Assistance | Stock | 0mph | 0mph |
| Volkswagen| T-Roc 2021<sup>4</sup> | Driver Assistance | Stock | 0mph | 0mph |
| Volkswagen| Taos 2022<sup>4</sup> | Driver Assistance | Stock | 0mph | 0mph |
| Volkswagen| Tiguan 2020 | Driver Assistance | Stock | 0mph | 0mph |
| Volkswagen| Touran 2017 | Driver Assistance | Stock | 0mph | 0mph |
| Make | Model (US Market Reference) | Supported Package | ACC | No ACC accel below | No ALC below |
| ----------| --------------------------------| ------------------| -----------------| -------------------| -------------|
| Audi | A3 2014-19 | ACC + Lane Assist | Stock | 0mph | 0mph |
| Audi | A3 Sportback e-tron 2017-18 | ACC + Lane Assist | Stock | 0mph | 0mph |
| Audi | Q2 2018 | ACC + Lane Assist | Stock | 0mph | 0mph |
| Audi | Q3 2020-21 | ACC + Lane Assist | Stock | 0mph | 0mph |
| Audi | S3 2015 | ACC + Lane Assist | Stock | 0mph | 0mph |
| Cadillac | Escalade ESV 2016<sup>1</sup> | ACC + LKAS | openpilot | 0mph | 7mph |
| Chevrolet | Volt 2017-18<sup>1</sup> | Adaptive Cruise | openpilot | 0mph | 7mph |
| Chrysler | Pacifica 2017-18 | Adaptive Cruise | Stock | 0mph | 9mph |
| Chrysler | Pacifica 2020 | Adaptive Cruise | Stock | 0mph | 39mph |
| Chrysler | Pacifica Hybrid 2017-18 | Adaptive Cruise | Stock | 0mph | 9mph |
| Chrysler | Pacifica Hybrid 2019-21 | Adaptive Cruise | Stock | 0mph | 39mph |
| Genesis | G70 2018 | All | Stock | 0mph | 0mph |
| Genesis | G70 2020 | All | Stock | 0mph | 0mph |
| Genesis | G80 2018 | All | Stock | 0mph | 0mph |
| Genesis | G90 2018 | All | Stock | 0mph | 0mph |
| GMC | Acadia 2018<sup>1</sup> | Adaptive Cruise | openpilot | 0mph | 7mph |
| Hyundai | Elantra 2017-19 | SCC + LKAS | Stock | 19mph | 34mph |
| Hyundai | Elantra 2021 | SCC + LKAS | Stock | 0mph | 0mph |
| Hyundai | Elantra Hybrid 2021 | SCC + LKAS | Stock | 0mph | 0mph |
| Hyundai | Genesis 2015-16 | SCC + LKAS | Stock | 19mph | 37mph |
| Hyundai | Ioniq Electric 2019 | SCC + LKAS | Stock | 0mph | 32mph |
| Hyundai | Ioniq Electric 2020 | SCC + LKAS | Stock | 0mph | 0mph |
| Hyundai | Ioniq Hybrid 2017-19 | SCC + LKAS | Stock | 0mph | 32mph |
| Hyundai | Ioniq Hybrid 2020-22 | SCC + LFA | Stock | 0mph | 0mph |
| Hyundai | Ioniq PHEV 2020-21 | SCC + LKAS | Stock | 0mph | 0mph |
| Hyundai | Kona 2020 | SCC + LKAS | Stock | 0mph | 0mph |
| Hyundai | Kona EV 2018-19 | SCC + LKAS | Stock | 0mph | 0mph |
| Hyundai | Kona Hybrid 2020 | SCC + LKAS | Stock | 0mph | 0mph |
| Hyundai | Santa Fe 2019-20 | All | Stock | 0mph | 0mph |
| Hyundai | Santa Fe 2021-22 | All | Stock | 0mph | 0mph |
| Hyundai | Santa Fe Hybrid 2022 | All | Stock | 0mph | 0mph |
| Hyundai | Santa Fe Plug-in Hybrid 2022 | All | Stock | 0mph | 0mph |
| Hyundai | Sonata 2018-2019 | SCC + LKAS | Stock | 0mph | 0mph |
| Hyundai | Sonata Hybrid 2021-22 | All | Stock | 0mph | 0mph |
| Hyundai | Veloster 2019-20 | SCC + LKAS | Stock | 5mph | 0mph |
| Jeep | Grand Cherokee 2016-18 | Adaptive Cruise | Stock | 0mph | 9mph |
| Jeep | Grand Cherokee 2019-20 | Adaptive Cruise | Stock | 0mph | 39mph |
| Kia | Ceed 2019 | SCC + LKAS | Stock | 0mph | 0mph |
| Kia | Forte 2018-21 | SCC + LKAS | Stock | 0mph | 0mph |
| Kia | K5 2021-22 | SCC + LFA | Stock | 0mph | 0mph |
| Kia | Niro EV 2019-22 | All | Stock | 0mph | 0mph |
| Kia | Niro Hybrid 2021 | SCC + LKAS | Stock | 0mph | 0mph |
| Kia | Niro PHEV 2019 | SCC + LKAS | Stock | 10mph | 32mph |
| Kia | Optima 2017 | SCC + LKAS | Stock | 0mph | 32mph |
| Kia | Optima 2019 | SCC + LKAS | Stock | 0mph | 0mph |
| Kia | Seltos 2021 | SCC + LKAS | Stock | 0mph | 0mph |
| Kia | Sorento 2018-19 | SCC + LKAS | Stock | 0mph | 0mph |
| Kia | Stinger 2018 | SCC + LKAS | Stock | 0mph | 0mph |
| Kia | Telluride 2020 | SCC + LKAS | Stock | 0mph | 0mph |
| Mazda | CX-5 2022 | All | Stock | 0mph | 0mph |
| Mazda | CX-9 2021 | All | Stock | 0mph | 28mph |
| Nissan | Altima 2019-20 | ProPILOT | Stock | 0mph | 0mph |
| Nissan | Leaf 2018-22 | ProPILOT | Stock | 0mph | 0mph |
| Nissan | Rogue 2018-20 | ProPILOT | Stock | 0mph | 0mph |
| Nissan | X-Trail 2017 | ProPILOT | Stock | 0mph | 0mph |
| SEAT | Ateca 2018 | Driver Assistance | Stock | 0mph | 0mph |
| SEAT | Leon 2014-2020 | Driver Assistance | Stock | 0mph | 0mph |
| Subaru | Ascent 2019 | EyeSight | Stock | 0mph | 0mph |
| Subaru | Crosstrek 2018-20 | EyeSight | Stock | 0mph | 0mph |
| Subaru | Forester 2019-21 | EyeSight | Stock | 0mph | 0mph |
| Subaru | Impreza 2017-19 | EyeSight | Stock | 0mph | 0mph |
| Škoda | Kamiq 2021<sup>2</sup> | Driver Assistance | Stock | 0mph | 0mph |
| Škoda | Karoq 2019 | Driver Assistance | Stock | 0mph | 0mph |
| Škoda | Kodiaq 2018-19 | Driver Assistance | Stock | 0mph | 0mph |
| Škoda | Octavia 2015, 2018-19 | Driver Assistance | Stock | 0mph | 0mph |
| Škoda | Octavia RS 2016 | Driver Assistance | Stock | 0mph | 0mph |
| Škoda | Scala 2020 | Driver Assistance | Stock | 0mph | 0mph |
| Škoda | Superb 2015-18 | Driver Assistance | Stock | 0mph | 0mph |
| Volkswagen| Arteon 2018, 2021<sup>4</sup> | Driver Assistance | Stock | 0mph | 0mph |
| Volkswagen| Atlas 2018-19, 2022<sup>4</sup> | Driver Assistance | Stock | 0mph | 0mph |
| Volkswagen| California 2021<sup>4</sup> | Driver Assistance | Stock | 0mph | 32mph |
| Volkswagen| e-Golf 2014, 2019-20 | Driver Assistance | Stock | 0mph | 0mph |
| Volkswagen| Golf 2015-20 | Driver Assistance | Stock | 0mph | 0mph |
| Volkswagen| Golf Alltrack 2017-18 | Driver Assistance | Stock | 0mph | 0mph |
| Volkswagen| Golf GTE 2016 | Driver Assistance | Stock | 0mph | 0mph |
| Volkswagen| Golf GTI 2018-20 | Driver Assistance | Stock | 0mph | 0mph |
| Volkswagen| Golf R 2016-19 | Driver Assistance | Stock | 0mph | 0mph |
| Volkswagen| Golf SportsVan 2016 | Driver Assistance | Stock | 0mph | 0mph |
| Volkswagen| Golf SportWagen 2015 | Driver Assistance | Stock | 0mph | 0mph |
| Volkswagen| Jetta 2018-20 | Driver Assistance | Stock | 0mph | 0mph |
| Volkswagen| Jetta GLI 2021 | Driver Assistance | Stock | 0mph | 0mph |
| Volkswagen| Passat 2016-18<sup>3</sup> | Driver Assistance | Stock | 0mph | 0mph |
| Volkswagen| Polo 2020 | Driver Assistance | Stock | 0mph | 0mph |
| Volkswagen| T-Cross 2021<sup>4</sup> | Driver Assistance | Stock | 0mph | 0mph |
| Volkswagen| T-Roc 2021<sup>4</sup> | Driver Assistance | Stock | 0mph | 0mph |
| Volkswagen| Taos 2022<sup>4</sup> | Driver Assistance | Stock | 0mph | 0mph |
| Volkswagen| Tiguan 2020 | Driver Assistance | Stock | 0mph | 0mph |
| Volkswagen| Touran 2017 | Driver Assistance | Stock | 0mph | 0mph |
<sup>1</sup>Requires an [OBD-II car harness](https://comma.ai/shop/products/comma-car-harness) and [community built ASCM harness](https://github.com/commaai/openpilot/wiki/GM#hardware). ***NOTE: disconnecting the ASCM disables Automatic Emergency Braking (AEB).*** <br />
<sup>2</sup>Not including the China market Kamiq, which is based on the (currently) unsupported PQ34 platform. <br />

2
launch_env.sh
Unescape View File

@ -7,7 +7,7 @@ export OPENBLAS_NUM_THREADS=1
export VECLIB_MAXIMUM_THREADS=1
if [ -z "$REQUIRED_NEOS_VERSION" ]; then
export REQUIRED_NEOS_VERSION="18"
export REQUIRED_NEOS_VERSION="19.1"
fi
if [ -z "$AGNOS_VERSION" ]; then

5
pyextra/acados_template/acados_ocp_solver_pyx.pyx
Unescape View File

@ -417,8 +417,9 @@ cdef class AcadosOcpSolverFast:
string_value = value_.encode('utf-8')
acados_solver_common.ocp_nlp_solver_opts_set(self.nlp_config, self.nlp_opts, field, <void *> &string_value[0])
raise Exception('AcadosOcpSolver.options_set() does not support field {}.'\
'\n Possible values are {}.'.format(field_, ', '.join(int_fields + double_fields + string_fields)))
else:
raise Exception('AcadosOcpSolver.options_set() does not support field {}.'\
'\n Possible values are {}.'.format(field_, ', '.join(int_fields + double_fields + string_fields)))
def __del__(self):

42
release/files_common
Unescape View File

@ -70,7 +70,7 @@ selfdrive/version.py
selfdrive/__init__.py
selfdrive/config.py
selfdrive/crash.py
selfdrive/sentry.py
selfdrive/swaglog.py
selfdrive/logmessaged.py
selfdrive/tombstoned.py
@ -88,6 +88,8 @@ selfdrive/boardd/.gitignore
selfdrive/boardd/SConscript
selfdrive/boardd/__init__.py
selfdrive/boardd/boardd.cc
selfdrive/boardd/boardd.h
selfdrive/boardd/main.cc
selfdrive/boardd/boardd.py
selfdrive/boardd/boardd_api_impl.pyx
selfdrive/boardd/can_list_to_can_capnp.cc
@ -198,6 +200,8 @@ selfdrive/common/version.h
selfdrive/common/swaglog.h
selfdrive/common/swaglog.cc
selfdrive/common/statlog.h
selfdrive/common/statlog.cc
selfdrive/common/util.cc
selfdrive/common/util.h
selfdrive/common/queue.h
@ -228,17 +232,19 @@ selfdrive/controls/radard.py
selfdrive/controls/lib/__init__.py
selfdrive/controls/lib/alertmanager.py
selfdrive/controls/lib/alerts_offroad.json
selfdrive/controls/lib/events.py
selfdrive/controls/lib/desire_helper.py
selfdrive/controls/lib/drive_helpers.py
selfdrive/controls/lib/latcontrol_pid.py
selfdrive/controls/lib/events.py
selfdrive/controls/lib/lane_planner.py
selfdrive/controls/lib/latcontrol_angle.py
selfdrive/controls/lib/latcontrol_indi.py
selfdrive/controls/lib/latcontrol_lqr.py
selfdrive/controls/lib/latcontrol_angle.py
selfdrive/controls/lib/longcontrol.py
selfdrive/controls/lib/latcontrol_pid.py
selfdrive/controls/lib/latcontrol.py
selfdrive/controls/lib/lateral_planner.py
selfdrive/controls/lib/lane_planner.py
selfdrive/controls/lib/pid.py
selfdrive/controls/lib/longcontrol.py
selfdrive/controls/lib/longitudinal_planner.py
selfdrive/controls/lib/pid.py
selfdrive/controls/lib/radar_helpers.py
selfdrive/controls/lib/vehicle_model.py
@ -255,6 +261,7 @@ selfdrive/hardware/base.py
selfdrive/hardware/hw.h
selfdrive/hardware/eon/__init__.py
selfdrive/hardware/eon/androidd.py
selfdrive/hardware/eon/shutdownd.py
selfdrive/hardware/eon/hardware.h
selfdrive/hardware/eon/hardware.py
selfdrive/hardware/eon/neos.py
@ -549,7 +556,7 @@ opendbc/can/dbc_out/.gitignore
opendbc/chrysler_pacifica_2017_hybrid.dbc
opendbc/chrysler_pacifica_2017_hybrid_private_fusion.dbc
opendbc/gm_global_a_powertrain.dbc
opendbc/gm_global_a_powertrain_generated.dbc
opendbc/gm_global_a_object.dbc
opendbc/gm_global_a_chassis.dbc
@ -571,8 +578,6 @@ opendbc/honda_crv_hybrid_2019_can_generated.dbc
opendbc/honda_fit_ex_2018_can_generated.dbc
opendbc/honda_odyssey_exl_2018_generated.dbc
opendbc/honda_odyssey_extreme_edition_2018_china_can_generated.dbc
opendbc/honda_pilot_touring_2017_can_generated.dbc
opendbc/honda_ridgeline_black_edition_2017_can_generated.dbc
opendbc/honda_insight_ex_2019_can_generated.dbc
opendbc/acura_ilx_2016_nidec.dbc
@ -589,22 +594,9 @@ opendbc/subaru_outback_2015_generated.dbc
opendbc/subaru_outback_2019_generated.dbc
opendbc/subaru_forester_2017_generated.dbc
opendbc/toyota_rav4_hybrid_2017_pt_generated.dbc
opendbc/toyota_rav4_2017_pt_generated.dbc
opendbc/toyota_prius_2017_pt_generated.dbc
opendbc/toyota_corolla_2017_pt_generated.dbc
opendbc/lexus_rx_350_2016_pt_generated.dbc
opendbc/lexus_rx_hybrid_2017_pt_generated.dbc
opendbc/toyota_tnga_k_pt_generated.dbc
opendbc/toyota_new_mc_pt_generated.dbc
opendbc/toyota_nodsu_pt_generated.dbc
opendbc/toyota_nodsu_hybrid_pt_generated.dbc
opendbc/toyota_highlander_2017_pt_generated.dbc
opendbc/toyota_highlander_hybrid_2018_pt_generated.dbc
opendbc/toyota_avalon_2017_pt_generated.dbc
opendbc/toyota_sienna_xle_2018_pt_generated.dbc
opendbc/lexus_is_2018_pt_generated.dbc
opendbc/lexus_ct200h_2018_pt_generated.dbc
opendbc/lexus_nx300h_2018_pt_generated.dbc
opendbc/lexus_nx300_2018_pt_generated.dbc
opendbc/toyota_adas.dbc
opendbc/toyota_tss2_adas.dbc

47
selfdrive/athena/athenad.py
Unescape View File

@ -14,7 +14,7 @@ import time
import tempfile
from collections import namedtuple
from functools import partial
from typing import Any
from typing import Any, Dict
import requests
from jsonrpc import JSONRPCResponseManager, dispatcher
@ -55,7 +55,7 @@ log_recv_queue: Any = queue.Queue()
cancelled_uploads: Any = set()
UploadItem = namedtuple('UploadItem', ['path', 'url', 'headers', 'created_at', 'id', 'retry_count', 'current', 'progress'], defaults=(0, False, 0))
cur_upload_items = {}
cur_upload_items: Dict[int, Any] = {}
class UploadQueueCache():
@ -128,7 +128,26 @@ def jsonrpc_handler(end_event):
send_queue.put_nowait(json.dumps({"error": str(e)}))
def upload_handler(end_event):
def retry_upload(tid: int, end_event: threading.Event) -> None:
if cur_upload_items[tid].retry_count < MAX_RETRY_COUNT:
item = cur_upload_items[tid]
item = item._replace(
retry_count=item.retry_count + 1,
progress=0,
current=False
)
upload_queue.put_nowait(item)
UploadQueueCache.cache(upload_queue)
cur_upload_items[tid] = None
for _ in range(RETRY_DELAY):
time.sleep(1)
if end_event.is_set():
break
def upload_handler(end_event: threading.Event) -> None:
tid = threading.get_ident()
while not end_event.is_set():
@ -145,27 +164,15 @@ def upload_handler(end_event):
def cb(sz, cur):
cur_upload_items[tid] = cur_upload_items[tid]._replace(progress=cur / sz if sz else 1)
_do_upload(cur_upload_items[tid], cb)
response = _do_upload(cur_upload_items[tid], cb)
if response.status_code not in (200, 201, 403, 412):
cloudlog.warning(f"athena.upload_handler.retry {response.status_code} {cur_upload_items[tid]}")
retry_upload(tid, end_event)
UploadQueueCache.cache(upload_queue)
except (requests.exceptions.Timeout, requests.exceptions.ConnectionError, requests.exceptions.SSLError) as e:
cloudlog.warning(f"athena.upload_handler.retry {e} {cur_upload_items[tid]}")
if cur_upload_items[tid].retry_count < MAX_RETRY_COUNT:
item = cur_upload_items[tid]
item = item._replace(
retry_count=item.retry_count + 1,
progress=0,
current=False
)
upload_queue.put_nowait(item)
UploadQueueCache.cache(upload_queue)
cur_upload_items[tid] = None
for _ in range(RETRY_DELAY):
time.sleep(1)
if end_event.is_set():
break
retry_upload(tid, end_event)
except queue.Empty:
pass

9
selfdrive/athena/registration.py
Unescape View File

@ -10,13 +10,18 @@ from common.params import Params
from common.spinner import Spinner
from common.basedir import PERSIST
from selfdrive.controls.lib.alertmanager import set_offroad_alert
from selfdrive.hardware import HARDWARE
from selfdrive.hardware import HARDWARE, PC
from selfdrive.swaglog import cloudlog
UNREGISTERED_DONGLE_ID = "UnregisteredDevice"
def is_registered_device() -> bool:
dongle = Params().get("DongleId", encoding='utf-8')
return dongle not in (None, UNREGISTERED_DONGLE_ID)
def register(show_spinner=False) -> str:
params = Params()
params.put("SubscriberInfo", HARDWARE.get_subscriber_info())
@ -86,7 +91,7 @@ def register(show_spinner=False) -> str:
if dongle_id:
params.put("DongleId", dongle_id)
set_offroad_alert("Offroad_UnofficialHardware", dongle_id == UNREGISTERED_DONGLE_ID)
set_offroad_alert("Offroad_UnofficialHardware", (dongle_id == UNREGISTERED_DONGLE_ID) and not PC)
return dongle_id

25
selfdrive/athena/tests/test_athenad.py
Unescape View File

@ -166,6 +166,31 @@ class TestAthenadMethods(unittest.TestCase):
finally:
end_event.set()
@with_http_server
@mock.patch('requests.put')
def test_upload_handler_retry(self, host, mock_put):
for status, retry in ((500, True), (412, False)):
mock_put.return_value.status_code = status
fn = os.path.join(athenad.ROOT, 'qlog.bz2')
Path(fn).touch()
item = athenad.UploadItem(path=fn, url=f"{host}/qlog.bz2", headers={}, created_at=int(time.time()*1000), id='')
end_event = threading.Event()
thread = threading.Thread(target=athenad.upload_handler, args=(end_event,))
thread.start()
athenad.upload_queue.put_nowait(item)
try:
self.wait_for_upload()
time.sleep(0.1)
self.assertEqual(athenad.upload_queue.qsize(), 1 if retry else 0)
finally:
end_event.set()
if retry:
self.assertEqual(athenad.upload_queue.get().retry_count, 1)
def test_upload_handler_timeout(self):
"""When an upload times out or fails to connect it should be placed back in the queue"""
fn = os.path.join(athenad.ROOT, 'qlog.bz2')

2
selfdrive/boardd/SConscript
Unescape View File

@ -1,7 +1,7 @@
Import('env', 'envCython', 'common', 'cereal', 'messaging')
libs = ['usb-1.0', common, cereal, messaging, 'pthread', 'zmq', 'capnp', 'kj']
env.Program('boardd', ['boardd.cc', 'panda.cc', 'pigeon.cc'], LIBS=libs)
env.Program('boardd', ['main.cc', 'boardd.cc', 'panda.cc', 'pigeon.cc'], LIBS=libs)
env.Library('libcan_list_to_can_capnp', ['can_list_to_can_capnp.cc'])
envCython.Program('boardd_api_impl.so', 'boardd_api_impl.pyx', LIBS=["can_list_to_can_capnp", 'capnp', 'kj'] + envCython["LIBS"])

128
selfdrive/boardd/boardd.cc
Unescape View File

@ -1,3 +1,5 @@
#include "selfdrive/boardd/boardd.h"
#include <sched.h>
#include <sys/cdefs.h>
#include <sys/resource.h>
@ -10,12 +12,12 @@
#include <cassert>
#include <cerrno>
#include <chrono>
#include <cmath>
#include <cstdint>
#include <cstdio>
#include <cstdlib>
#include <future>
#include <thread>
#include <unordered_map>
#include <libusb-1.0/libusb.h>
@ -26,9 +28,7 @@
#include "selfdrive/common/timing.h"
#include "selfdrive/common/util.h"
#include "selfdrive/hardware/hw.h"
#include "selfdrive/locationd/ublox_msg.h"
#include "selfdrive/boardd/panda.h"
#include "selfdrive/boardd/pigeon.h"
// -- Multi-panda conventions --
@ -161,7 +161,8 @@ bool safety_setter_thread(std::vector<Panda *> pandas) {
int safety_param;
auto safety_configs = car_params.getSafetyConfigs();
for (uint32_t i=0; i<pandas.size(); i++) {
uint16_t unsafe_mode = car_params.getUnsafeMode();
for (uint32_t i = 0; i < pandas.size(); i++) {
auto panda = pandas[i];
if (safety_configs.size() > i) {
@ -173,9 +174,8 @@ bool safety_setter_thread(std::vector<Panda *> pandas) {
safety_param = 0;
}
LOGW("panda %d: setting safety model: %d with param %d", i, (int)safety_model, safety_param);
panda->set_unsafe_mode(0); // see safety_declarations.h for allowed values
LOGW("panda %d: setting safety model: %d, param: %d, unsafe mode: %d", i, (int)safety_model, safety_param, unsafe_mode);
panda->set_unsafe_mode(unsafe_mode);
panda->set_safety_model(safety_model, safety_param);
}
@ -304,34 +304,34 @@ bool send_panda_states(PubMaster *pm, const std::vector<Panda *> &pandas, bool s
std::vector<health_t> pandaStates;
for (const auto& panda : pandas){
health_t pandaState = panda->get_state();
health_t health = panda->get_state();
if (spoofing_started) {
pandaState.ignition_line = 1;
health.ignition_line_pkt = 1;
}
ignition_local |= ((pandaState.ignition_line != 0) || (pandaState.ignition_can != 0));
ignition_local |= ((health.ignition_line_pkt != 0) || (health.ignition_can_pkt != 0));
pandaStates.push_back(pandaState);
pandaStates.push_back(health);
}
for (uint32_t i=0; i<pandas.size(); i++) {
for (uint32_t i = 0; i < pandas.size(); i++) {
auto panda = pandas[i];
const auto &pandaState = pandaStates[i];
const auto &health = pandaStates[i];
// Make sure CAN buses are live: safety_setter_thread does not work if Panda CAN are silent and there is only one other CAN node
if (pandaState.safety_model == (uint8_t)(cereal::CarParams::SafetyModel::SILENT)) {
if (health.safety_mode_pkt == (uint8_t)(cereal::CarParams::SafetyModel::SILENT)) {
panda->set_safety_model(cereal::CarParams::SafetyModel::NO_OUTPUT);
}
#ifndef __x86_64__
bool power_save_desired = !ignition_local && !pigeon_active;
if (pandaState.power_save_enabled != power_save_desired) {
if (health.power_save_enabled_pkt != power_save_desired) {
panda->set_power_saving(power_save_desired);
}
// set safety mode to NO_OUTPUT when car is off. ELM327 is an alternative if we want to leverage athenad/connect
if (!ignition_local && (pandaState.safety_model != (uint8_t)(cereal::CarParams::SafetyModel::NO_OUTPUT))) {
if (!ignition_local && (health.safety_mode_pkt != (uint8_t)(cereal::CarParams::SafetyModel::NO_OUTPUT))) {
panda->set_safety_model(cereal::CarParams::SafetyModel::NO_OUTPUT);
}
#endif
@ -341,25 +341,27 @@ bool send_panda_states(PubMaster *pm, const std::vector<Panda *> &pandas, bool s
}
auto ps = pss[i];
ps.setUptime(pandaState.uptime);
ps.setIgnitionLine(pandaState.ignition_line);
ps.setIgnitionCan(pandaState.ignition_can);
ps.setControlsAllowed(pandaState.controls_allowed);
ps.setGasInterceptorDetected(pandaState.gas_interceptor_detected);
ps.setCanRxErrs(pandaState.can_rx_errs);
ps.setCanSendErrs(pandaState.can_send_errs);
ps.setCanFwdErrs(pandaState.can_fwd_errs);
ps.setGmlanSendErrs(pandaState.gmlan_send_errs);
ps.setUptime(health.uptime_pkt);
ps.setBlockedCnt(health.blocked_msg_cnt_pkt);
ps.setIgnitionLine(health.ignition_line_pkt);
ps.setIgnitionCan(health.ignition_can_pkt);
ps.setControlsAllowed(health.controls_allowed_pkt);
ps.setGasInterceptorDetected(health.gas_interceptor_detected_pkt);
ps.setCanRxErrs(health.can_rx_errs_pkt);
ps.setCanSendErrs(health.can_send_errs_pkt);
ps.setCanFwdErrs(health.can_fwd_errs_pkt);
ps.setGmlanSendErrs(health.gmlan_send_errs_pkt);
ps.setPandaType(panda->hw_type);
ps.setSafetyModel(cereal::CarParams::SafetyModel(pandaState.safety_model));
ps.setSafetyParam(pandaState.safety_param);
ps.setFaultStatus(cereal::PandaState::FaultStatus(pandaState.fault_status));
ps.setPowerSaveEnabled((bool)(pandaState.power_save_enabled));
ps.setHeartbeatLost((bool)(pandaState.heartbeat_lost));
ps.setHarnessStatus(cereal::PandaState::HarnessStatus(pandaState.car_harness_status));
ps.setSafetyModel(cereal::CarParams::SafetyModel(health.safety_mode_pkt));
ps.setSafetyParam(health.safety_param_pkt);
ps.setFaultStatus(cereal::PandaState::FaultStatus(health.fault_status_pkt));
ps.setPowerSaveEnabled((bool)(health.power_save_enabled_pkt));
ps.setHeartbeatLost((bool)(health.heartbeat_lost_pkt));
ps.setUnsafeMode(health.unsafe_mode_pkt);
ps.setHarnessStatus(cereal::PandaState::HarnessStatus(health.car_harness_status_pkt));
// Convert faults bitset to capnp list
std::bitset<sizeof(pandaState.faults) * 8> fault_bits(pandaState.faults);
std::bitset<sizeof(health.faults_pkt) * 8> fault_bits(health.faults_pkt);
auto faults = ps.initFaults(fault_bits.count());
size_t j = 0;
@ -396,12 +398,12 @@ void send_peripheral_state(PubMaster *pm, Panda *panda) {
LOGW("reading hwmon took %lfms", read_time);
}
} else {
ps.setVoltage(pandaState.voltage);
ps.setCurrent(pandaState.current);
ps.setVoltage(pandaState.voltage_pkt);
ps.setCurrent(pandaState.current_pkt);
}
uint16_t fan_speed_rpm = panda->get_fan_speed();
ps.setUsbPowerMode(cereal::PeripheralState::UsbPowerMode(pandaState.usb_power_mode));
ps.setUsbPowerMode(cereal::PeripheralState::UsbPowerMode(pandaState.usb_power_mode_pkt));
ps.setFanSpeedRpm(fan_speed_rpm);
pm->send("peripheralState", msg);
@ -421,6 +423,8 @@ void panda_state_thread(PubMaster *pm, std::vector<Panda *> pandas, bool spoofin
// run at 2hz
while (!do_exit && check_all_connected(pandas)) {
uint64_t start_time = nanos_since_boot();
// send out peripheralState
send_peripheral_state(pm, peripheral_panda);
ignition = send_panda_states(pm, pandas, spoofing_started);
@ -453,7 +457,9 @@ void panda_state_thread(PubMaster *pm, std::vector<Panda *> pandas, bool spoofin
for (const auto &panda : pandas) {
panda->send_heartbeat(engaged);
}
util::sleep_for(500);
uint64_t dt = nanos_since_boot() - start_time;
util::sleep_for(500 - dt / 1000000ULL);
}
}
@ -552,38 +558,11 @@ void pigeon_thread(Panda *panda) {
std::unique_ptr<Pigeon> pigeon(Hardware::TICI() ? Pigeon::connect("/dev/ttyHS0") : Pigeon::connect(panda));
std::unordered_map<char, uint64_t> last_recv_time;
std::unordered_map<char, int64_t> cls_max_dt = {
{(char)ublox::CLASS_NAV, int64_t(900000000ULL)}, // 0.9s
{(char)ublox::CLASS_RXM, int64_t(900000000ULL)}, // 0.9s
};
while (!do_exit && panda->connected) {
bool need_reset = false;
bool ignition_local = ignition;
std::string recv = pigeon->receive();
// Parse message header
if (ignition_local && recv.length() >= 3) {
if (recv[0] == (char)ublox::PREAMBLE1 && recv[1] == (char)ublox::PREAMBLE2) {
const char msg_cls = recv[2];
uint64_t t = nanos_since_boot();
if (t > last_recv_time[msg_cls]) {
last_recv_time[msg_cls] = t;
}
}
}
// Check based on message frequency
for (const auto& [msg_cls, max_dt] : cls_max_dt) {
int64_t dt = (int64_t)nanos_since_boot() - (int64_t)last_recv_time[msg_cls];
if (ignition_last && ignition_local && dt > max_dt) {
LOGD("ublox receive timeout, msg class: 0x%02x, dt %llu", msg_cls, dt);
// TODO: turn on reset after verification of logs
// need_reset = true;
}
}
// Check based on null bytes
if (ignition_local && recv.length() > 0 && recv[0] == (char)0x00) {
need_reset = true;
@ -599,12 +578,6 @@ void pigeon_thread(Panda *panda) {
if((ignition_local && !ignition_last) || need_reset) {
pigeon_active = true;
pigeon->init();
// Set receive times to current time
uint64_t t = nanos_since_boot() + 10000000000ULL; // Give ublox 10 seconds to start
for (const auto& [msg_cls, dt] : cls_max_dt) {
last_recv_time[msg_cls] = t;
}
} else if (!ignition_local && ignition_last) {
// power off on falling edge of ignition
LOGD("powering off pigeon\n");
@ -620,22 +593,11 @@ void pigeon_thread(Panda *panda) {
}
}
int main(int argc, char *argv[]) {
LOGW("starting boardd");
if (!Hardware::PC()) {
int err;
err = util::set_realtime_priority(54);
assert(err == 0);
err = util::set_core_affinity({Hardware::TICI() ? 4 : 3});
assert(err == 0);
}
void boardd_main_thread(std::vector<std::string> serials) {
if (serials.size() == 0) serials.push_back("");
LOGW("attempting to connect");
PubMaster pm({"pandaStates", "peripheralState"});
std::vector<std::string> serials(argv + 1, argv + argc);
if (serials.size() == 0) serials.push_back("");
LOGW("attempting to connect");
// connect to all provided serials
std::vector<Panda *> pandas;

6
selfdrive/boardd/boardd.h
Unescape View File

@ -0,0 +1,6 @@
#pragma once
#include "selfdrive/boardd/panda.h"
bool safety_setter_thread(std::vector<Panda *> pandas);
void boardd_main_thread(std::vector<std::string> serials);

22
selfdrive/boardd/main.cc
Unescape View File

@ -0,0 +1,22 @@
#include <cassert>
#include "selfdrive/boardd/boardd.h"
#include "selfdrive/common/swaglog.h"
#include "selfdrive/common/util.h"
#include "selfdrive/hardware/hw.h"
int main(int argc, char *argv[]) {
LOGW("starting boardd");
if (!Hardware::PC()) {
int err;
err = util::set_realtime_priority(54);
assert(err == 0);
err = util::set_core_affinity({Hardware::TICI() ? 4 : 3});
assert(err == 0);
}
std::vector<std::string> serials(argv + 1, argv + argc);
boardd_main_thread(serials);
return 0;
}

14
selfdrive/boardd/panda.cc
Unescape View File

@ -45,11 +45,11 @@ Panda::Panda(std::string serial, uint32_t bus_offset) : bus_offset(bus_offset) {
libusb_device_descriptor desc;
libusb_get_device_descriptor(dev_list[i], &desc);
if (desc.idVendor == 0xbbaa && desc.idProduct == 0xddcc) {
libusb_open(dev_list[i], &dev_handle);
if (dev_handle == NULL) { goto fail; }
int ret = libusb_open(dev_list[i], &dev_handle);
if (dev_handle == NULL || ret < 0) { goto fail; }
unsigned char desc_serial[26] = { 0 };
int ret = libusb_get_string_descriptor_ascii(dev_handle, desc.iSerialNumber, desc_serial, std::size(desc_serial));
ret = libusb_get_string_descriptor_ascii(dev_handle, desc.iSerialNumber, desc_serial, std::size(desc_serial));
if (ret < 0) { goto fail; }
usb_serial = std::string((char *)desc_serial, ret).c_str();
@ -130,12 +130,14 @@ std::vector<std::string> Panda::list() {
libusb_get_device_descriptor(device, &desc);
if (desc.idVendor == 0xbbaa && desc.idProduct == 0xddcc) {
libusb_device_handle *handle = NULL;
libusb_open(device, &handle);
int ret = libusb_open(device, &handle);
if (ret < 0) { goto finish; }
unsigned char desc_serial[26] = { 0 };
int ret = libusb_get_string_descriptor_ascii(handle, desc.iSerialNumber, desc_serial, std::size(desc_serial));
ret = libusb_get_string_descriptor_ascii(handle, desc.iSerialNumber, desc_serial, std::size(desc_serial));
libusb_close(handle);
if (ret < 0) { goto finish; }
serials.push_back(std::string((char *)desc_serial, ret).c_str());
}
}

24
selfdrive/boardd/panda.h
Unescape View File

@ -13,6 +13,7 @@
#include "cereal/gen/cpp/car.capnp.h"
#include "cereal/gen/cpp/log.capnp.h"
#include "panda/board/health.h"
#define TIMEOUT 0
#define PANDA_BUS_CNT 4
@ -24,29 +25,6 @@
#define CANPACKET_REJECTED (0xC0U)
#define CANPACKET_RETURNED (0x80U)
// copied from panda/board/main.c
struct __attribute__((packed)) health_t {
uint32_t uptime;
uint32_t voltage;
uint32_t current;
uint32_t can_rx_errs;
uint32_t can_send_errs;
uint32_t can_fwd_errs;
uint32_t gmlan_send_errs;
uint32_t faults;
uint8_t ignition_line;
uint8_t ignition_can;
uint8_t controls_allowed;
uint8_t gas_interceptor_detected;
uint8_t car_harness_status;
uint8_t usb_power_mode;
uint8_t safety_model;
int16_t safety_param;
uint8_t fault_status;
uint8_t power_save_enabled;
uint8_t heartbeat_lost;
};
struct __attribute__((packed)) can_header {
uint8_t reserved : 1;
uint8_t bus : 3;

29
selfdrive/camerad/cameras/camera_common.cc
Unescape View File

@ -19,6 +19,7 @@
#include "selfdrive/hardware/hw.h"
#ifdef QCOM
#include "CL/cl_ext_qcom.h"
#include "selfdrive/camerad/cameras/camera_qcom.h"
#elif QCOM2
#include "selfdrive/camerad/cameras/camera_qcom2.h"
@ -28,6 +29,8 @@
#include "selfdrive/camerad/cameras/camera_replay.h"
#endif
ExitHandler do_exit;
class Debayer {
public:
Debayer(cl_device_id device_id, cl_context context, const CameraBuf *b, const CameraState *s) {
@ -339,8 +342,6 @@ float set_exposure_target(const CameraBuf *b, int x_start, int x_end, int x_skip
return lum_med / 256.0;
}
extern ExitHandler do_exit;
void *processing_thread(MultiCameraState *cameras, CameraState *cs, process_thread_cb callback) {
const char *thread_name = nullptr;
if (cs == &cameras->road_cam) {
@ -422,3 +423,27 @@ void common_process_driver_camera(MultiCameraState *s, CameraState *c, int cnt)
}
s->pm->send("driverCameraState", msg);
}
void camerad_thread() {
cl_device_id device_id = cl_get_device_id(CL_DEVICE_TYPE_DEFAULT);
// TODO: do this for QCOM2 too
#if defined(QCOM)
const cl_context_properties props[] = {CL_CONTEXT_PRIORITY_HINT_QCOM, CL_PRIORITY_HINT_HIGH_QCOM, 0};
cl_context context = CL_CHECK_ERR(clCreateContext(props, 1, &device_id, NULL, NULL, &err));
#else
cl_context context = CL_CHECK_ERR(clCreateContext(NULL, 1, &device_id, NULL, NULL, &err));
#endif
MultiCameraState cameras = {};
VisionIpcServer vipc_server("camerad", device_id, context);
cameras_init(&vipc_server, &cameras, device_id, context);
cameras_open(&cameras);
vipc_server.start_listener();
cameras_run(&cameras);
CL_CHECK(clReleaseContext(context));
}

6
selfdrive/camerad/cameras/camera_common.h
Unescape View File

@ -41,6 +41,11 @@ const bool env_send_driver = getenv("SEND_DRIVER") != NULL;
const bool env_send_road = getenv("SEND_ROAD") != NULL;
const bool env_send_wide_road = getenv("SEND_WIDE_ROAD") != NULL;
// for debugging
// note: ONLY_ROAD doesn't work, likely due to a mixup with wideRoad cam in the kernel
const bool env_only_driver = getenv("ONLY_DRIVER") != NULL;
const bool env_debug_frames = getenv("DEBUG_FRAMES") != NULL;
typedef void (*release_cb)(void *cookie, int buf_idx);
typedef struct CameraInfo {
@ -129,3 +134,4 @@ void cameras_open(MultiCameraState *s);
void cameras_run(MultiCameraState *s);
void cameras_close(MultiCameraState *s);
void camera_autoexposure(CameraState *s, float grey_frac);
void camerad_thread();

166
selfdrive/camerad/cameras/camera_qcom2.cc
Unescape View File

@ -227,7 +227,7 @@ void sensors_init(int video0_fd, int sensor_fd, int camera_num) {
buf_desc[0].size = buf_desc[0].length = sizeof(struct cam_cmd_i2c_info) + sizeof(struct cam_cmd_probe);
buf_desc[0].type = CAM_CMD_BUF_LEGACY;
struct cam_cmd_i2c_info *i2c_info = (struct cam_cmd_i2c_info *)alloc_w_mmu_hdl(video0_fd, buf_desc[0].size, (uint32_t*)&buf_desc[0].mem_handle);
struct cam_cmd_probe *probe = (struct cam_cmd_probe *)((uint8_t *)i2c_info) + sizeof(struct cam_cmd_i2c_info);
auto probe = (struct cam_cmd_probe *)(i2c_info + 1);
switch (camera_num) {
case 0:
@ -353,6 +353,9 @@ void config_isp(struct CameraState *s, int io_mem_handle, int fence, int request
struct cam_packet *pkt = (struct cam_packet *)alloc_w_mmu_hdl(s->multi_cam_state->video0_fd, size, &cam_packet_handle);
pkt->num_cmd_buf = 2;
pkt->kmd_cmd_buf_index = 0;
// YUV has kmd_cmd_buf_offset = 1780
// I guess this is the ISP command
// YUV also has patch_offset = 0x1030 and num_patches = 10
if (io_mem_handle != 0) {
pkt->io_configs_offset = sizeof(struct cam_cmd_buf_desc)*2;
@ -377,28 +380,65 @@ void config_isp(struct CameraState *s, int io_mem_handle, int fence, int request
buf_desc[0].mem_handle = buf0_mem_handle;
buf_desc[0].offset = buf0_offset;
// cam_isp_packet_generic_blob_handler
uint32_t tmp[] = {
// size is 0x20, type is 0(CAM_ISP_GENERIC_BLOB_TYPE_HFR_CONFIG)
0x2000,
0x1, 0x0, CAM_ISP_IFE_OUT_RES_RDI_0, 0x1, 0x0, 0x1, 0x0, 0x0, // 1 port, CAM_ISP_IFE_OUT_RES_RDI_0
// size is 0x38, type is 1(CAM_ISP_GENERIC_BLOB_TYPE_CLOCK_CONFIG), clocks
0x3801,
0x1, 0x4, // Dual mode, 4 RDI wires
0x18148d00, 0x0, 0x18148d00, 0x0, 0x18148d00, 0x0, // rdi clock
0x0, 0x0, 0x0, 0x0, 0x0, 0x0, // junk?
// offset 0x60
// size is 0xe0, type is 2(CAM_ISP_GENERIC_BLOB_TYPE_BW_CONFIG), bandwidth
0xe002,
0x1, 0x4, // 4 RDI
0x0, 0x0, 0x1ad27480, 0x0, 0x1ad27480, 0x0, // left_pix_vote
0x0, 0x0, 0x0, 0x0, 0x0, 0x0, // right_pix_vote
0x0, 0x0, 0x6ee11c0, 0x2, 0x6ee11c0, 0x2, // rdi_vote
0x0, 0x0, 0x0, 0x0,
0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0,
0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0,
0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0,
0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0};
// parsed by cam_isp_packet_generic_blob_handler
struct isp_packet {
uint32_t type_0;
cam_isp_resource_hfr_config resource_hfr;
uint32_t type_1;
cam_isp_clock_config clock;
uint64_t extra_rdi_hz[3];
uint32_t type_2;
cam_isp_bw_config bw;
struct cam_isp_bw_vote extra_rdi_vote[6];
} __attribute__((packed)) tmp;
memset(&tmp, 0, sizeof(tmp));
tmp.type_0 = CAM_ISP_GENERIC_BLOB_TYPE_HFR_CONFIG;
tmp.type_0 |= sizeof(cam_isp_resource_hfr_config) << 8;
static_assert(sizeof(cam_isp_resource_hfr_config) == 0x20);
tmp.resource_hfr = {
.num_ports = 1, // 10 for YUV (but I don't think we need them)
.port_hfr_config[0] = {
.resource_type = CAM_ISP_IFE_OUT_RES_RDI_0, // CAM_ISP_IFE_OUT_RES_FULL for YUV
.subsample_pattern = 1,
.subsample_period = 0,
.framedrop_pattern = 1,
.framedrop_period = 0,
}};
tmp.type_1 = CAM_ISP_GENERIC_BLOB_TYPE_CLOCK_CONFIG;
tmp.type_1 |= (sizeof(cam_isp_clock_config) + sizeof(tmp.extra_rdi_hz)) << 8;
static_assert((sizeof(cam_isp_clock_config) + sizeof(tmp.extra_rdi_hz)) == 0x38);
tmp.clock = {
.usage_type = 1, // dual mode
.num_rdi = 4,
.left_pix_hz = 404000000,
.right_pix_hz = 404000000,
.rdi_hz[0] = 404000000,
};
tmp.type_2 = CAM_ISP_GENERIC_BLOB_TYPE_BW_CONFIG;
tmp.type_2 |= (sizeof(cam_isp_bw_config) + sizeof(tmp.extra_rdi_vote)) << 8;
static_assert((sizeof(cam_isp_bw_config) + sizeof(tmp.extra_rdi_vote)) == 0xe0);
tmp.bw = {
.usage_type = 1, // dual mode
.num_rdi = 4,
.left_pix_vote = {
.resource_id = 0,
.cam_bw_bps = 450000000,
.ext_bw_bps = 450000000,
},
.rdi_vote[0] = {
.resource_id = 0,
.cam_bw_bps = 8706200000,
.ext_bw_bps = 8706200000,
},
};
static_assert(offsetof(struct isp_packet, type_2) == 0x60);
buf_desc[1].size = sizeof(tmp);
buf_desc[1].offset = io_mem_handle != 0 ? 0x60 : 0;
@ -406,7 +446,7 @@ void config_isp(struct CameraState *s, int io_mem_handle, int fence, int request
buf_desc[1].type = CAM_CMD_BUF_GENERIC;
buf_desc[1].meta_data = CAM_ISP_PACKET_META_GENERIC_BLOB_COMMON;
uint32_t *buf2 = (uint32_t *)alloc_w_mmu_hdl(s->multi_cam_state->video0_fd, buf_desc[1].size, (uint32_t*)&buf_desc[1].mem_handle, 0x20);
memcpy(buf2, tmp, sizeof(tmp));
memcpy(buf2, &tmp, sizeof(tmp));
if (io_mem_handle != 0) {
io_cfg[0].mem_handle[0] = io_mem_handle;
@ -415,19 +455,20 @@ void config_isp(struct CameraState *s, int io_mem_handle, int fence, int request
.height = FRAME_HEIGHT,
.plane_stride = FRAME_STRIDE,
.slice_height = FRAME_HEIGHT,
.meta_stride = 0x0,
.meta_stride = 0x0, // YUV has meta(stride=0x400, size=0x5000)
.meta_size = 0x0,
.meta_offset = 0x0,
.packer_config = 0x0,
.mode_config = 0x0,
.packer_config = 0x0, // 0xb for YUV
.mode_config = 0x0, // 0x9ef for YUV
.tile_config = 0x0,
.h_init = 0x0,
.v_init = 0x0,
};
io_cfg[0].format = CAM_FORMAT_MIPI_RAW_10;
io_cfg[0].color_pattern = 0x5;
io_cfg[0].bpp = 0xc;
io_cfg[0].resource_type = CAM_ISP_IFE_OUT_RES_RDI_0;
io_cfg[0].format = CAM_FORMAT_MIPI_RAW_10; // CAM_FORMAT_UBWC_TP10 for YUV
io_cfg[0].color_space = CAM_COLOR_SPACE_BASE; // CAM_COLOR_SPACE_BT601_FULL for YUV
io_cfg[0].color_pattern = 0x5; // 0x0 for YUV
io_cfg[0].bpp = 0xa;
io_cfg[0].resource_type = CAM_ISP_IFE_OUT_RES_RDI_0; // CAM_ISP_IFE_OUT_RES_FULL for YUV
io_cfg[0].fence = fence;
io_cfg[0].direction = CAM_BUF_OUTPUT;
io_cfg[0].subsample_pattern = 0x1;
@ -553,11 +594,7 @@ int open_v4l_by_name_and_index(const char name[], int index, int flags = O_RDWR
static void camera_open(CameraState *s) {
s->sensor_fd = open_v4l_by_name_and_index("cam-sensor-driver", s->camera_num);
assert(s->sensor_fd >= 0);
LOGD("opened sensor");
s->csiphy_fd = open_v4l_by_name_and_index("cam-csiphy-driver", s->camera_num);
assert(s->csiphy_fd >= 0);
LOGD("opened csiphy");
LOGD("opened sensor for %d", s->camera_num);
// probe the sensor
LOGD("-- Probing sensor %d", s->camera_num);
@ -605,7 +642,7 @@ static void camera_open(CameraState *s) {
.pixel_clk = 0x0,
.batch_size = 0x0,
.dsp_mode = 0x0,
.dsp_mode = CAM_ISP_DSP_MODE_NONE,
.hbi_cnt = 0x0,
.custom_csid = 0x0,
@ -630,6 +667,10 @@ static void camera_open(CameraState *s) {
s->isp_dev_handle = *isp_dev_handle;
LOGD("acquire isp dev");
s->csiphy_fd = open_v4l_by_name_and_index("cam-csiphy-driver", s->camera_num);
assert(s->csiphy_fd >= 0);
LOGD("opened csiphy for %d", s->camera_num);
struct cam_csiphy_acquire_dev_info csiphy_acquire_dev_info = {.combo_mode = 0};
auto csiphy_dev_handle = device_acquire(s->csiphy_fd, s->session_handle, &csiphy_acquire_dev_info);
assert(csiphy_dev_handle);
@ -645,6 +686,7 @@ static void camera_open(CameraState *s) {
//sensors_i2c(s, start_reg_array, std::size(start_reg_array), CAM_SENSOR_PACKET_OPCODE_SENSOR_STREAMON);
//sensors_i2c(s, stop_reg_array, std::size(stop_reg_array), CAM_SENSOR_PACKET_OPCODE_SENSOR_STREAMOFF);
// config csiphy
LOG("-- Config CSI PHY");
{
@ -686,8 +728,8 @@ static void camera_open(CameraState *s) {
req_mgr_link_info.dev_hdls[0] = s->isp_dev_handle;
req_mgr_link_info.dev_hdls[1] = s->sensor_dev_handle;
ret = cam_control(s->multi_cam_state->video0_fd, CAM_REQ_MGR_LINK, &req_mgr_link_info, sizeof(req_mgr_link_info));
LOGD("link: %d", ret);
s->link_handle = req_mgr_link_info.link_hdl;
LOGD("link: %d hdl: 0x%X", ret, s->link_handle);
struct cam_req_mgr_link_control req_mgr_link_control = {0};
req_mgr_link_control.ops = CAM_REQ_MGR_LINK_ACTIVATE;
@ -708,15 +750,17 @@ static void camera_open(CameraState *s) {
}
void cameras_init(VisionIpcServer *v, MultiCameraState *s, cl_device_id device_id, cl_context ctx) {
camera_init(s, v, &s->road_cam, CAMERA_ID_AR0231, 1, 20, device_id, ctx,
VISION_STREAM_RGB_BACK, VISION_STREAM_ROAD); // swap left/right
printf("road camera initted \n");
camera_init(s, v, &s->wide_road_cam, CAMERA_ID_AR0231, 0, 20, device_id, ctx,
VISION_STREAM_RGB_WIDE, VISION_STREAM_WIDE_ROAD);
printf("wide road camera initted \n");
camera_init(s, v, &s->driver_cam, CAMERA_ID_AR0231, 2, 20, device_id, ctx,
VISION_STREAM_RGB_FRONT, VISION_STREAM_DRIVER);
printf("driver camera initted \n");
if (!env_only_driver) {
camera_init(s, v, &s->road_cam, CAMERA_ID_AR0231, 1, 20, device_id, ctx,
VISION_STREAM_RGB_BACK, VISION_STREAM_ROAD); // swap left/right
printf("road camera initted \n");
camera_init(s, v, &s->wide_road_cam, CAMERA_ID_AR0231, 0, 20, device_id, ctx,
VISION_STREAM_RGB_WIDE, VISION_STREAM_WIDE_ROAD);
printf("wide road camera initted \n");
}
s->sm = new SubMaster({"driverState"});
s->pm = new PubMaster({"roadCameraState", "driverCameraState", "wideRoadCameraState", "thumbnail"});
@ -763,12 +807,14 @@ void cameras_open(MultiCameraState *s) {
ret = HANDLE_EINTR(ioctl(s->video0_fd, VIDIOC_SUBSCRIBE_EVENT, &sub));
printf("req mgr subscribe: %d\n", ret);
camera_open(&s->road_cam);
printf("road camera opened \n");
camera_open(&s->wide_road_cam);
printf("wide road camera opened \n");
camera_open(&s->driver_cam);
printf("driver camera opened \n");
if (!env_only_driver) {
camera_open(&s->road_cam);
printf("road camera opened \n");
camera_open(&s->wide_road_cam);
printf("wide road camera opened \n");
}
}
static void camera_close(CameraState *s) {
@ -816,9 +862,11 @@ static void camera_close(CameraState *s) {
}
void cameras_close(MultiCameraState *s) {
camera_close(&s->road_cam);
camera_close(&s->wide_road_cam);
camera_close(&s->driver_cam);
if (!env_only_driver) {
camera_close(&s->road_cam);
camera_close(&s->wide_road_cam);
}
delete s->sm;
delete s->pm;
@ -1010,16 +1058,20 @@ void process_road_camera(MultiCameraState *s, CameraState *c, int cnt) {
void cameras_run(MultiCameraState *s) {
LOG("-- Starting threads");
std::vector<std::thread> threads;
threads.push_back(start_process_thread(s, &s->road_cam, process_road_camera));
threads.push_back(start_process_thread(s, &s->driver_cam, common_process_driver_camera));
threads.push_back(start_process_thread(s, &s->wide_road_cam, process_road_camera));
if (!env_only_driver) {
threads.push_back(start_process_thread(s, &s->road_cam, process_road_camera));
threads.push_back(start_process_thread(s, &s->wide_road_cam, process_road_camera));
}
// start devices
LOG("-- Starting devices");
int start_reg_len = sizeof(start_reg_array) / sizeof(struct i2c_random_wr_payload);
sensors_i2c(&s->road_cam, start_reg_array, start_reg_len, CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG);
sensors_i2c(&s->wide_road_cam, start_reg_array, start_reg_len, CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG);
sensors_i2c(&s->driver_cam, start_reg_array, start_reg_len, CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG);
if (!env_only_driver) {
sensors_i2c(&s->road_cam, start_reg_array, start_reg_len, CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG);
sensors_i2c(&s->wide_road_cam, start_reg_array, start_reg_len, CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG);
}
// poll events
LOG("-- Dequeueing Video events");
@ -1043,8 +1095,10 @@ void cameras_run(MultiCameraState *s) {
if (ret == 0) {
if (ev.type == V4L_EVENT_CAM_REQ_MGR_EVENT) {
struct cam_req_mgr_message *event_data = (struct cam_req_mgr_message *)ev.u.data;
// LOGD("v4l2 event: sess_hdl %d, link_hdl %d, frame_id %d, req_id %lld, timestamp 0x%llx, sof_status %d\n", event_data->session_hdl, event_data->u.frame_msg.link_hdl, event_data->u.frame_msg.frame_id, event_data->u.frame_msg.request_id, event_data->u.frame_msg.timestamp, event_data->u.frame_msg.sof_status);
// printf("sess_hdl %d, link_hdl %d, frame_id %lu, req_id %lu, timestamp 0x%lx, sof_status %d\n", event_data->session_hdl, event_data->u.frame_msg.link_hdl, event_data->u.frame_msg.frame_id, event_data->u.frame_msg.request_id, event_data->u.frame_msg.timestamp, event_data->u.frame_msg.sof_status);
// LOGD("v4l2 event: sess_hdl 0x%X, link_hdl 0x%X, frame_id %d, req_id %lld, timestamp 0x%llx, sof_status %d\n", event_data->session_hdl, event_data->u.frame_msg.link_hdl, event_data->u.frame_msg.frame_id, event_data->u.frame_msg.request_id, event_data->u.frame_msg.timestamp, event_data->u.frame_msg.sof_status);
if (env_debug_frames) {
printf("sess_hdl 0x%X, link_hdl 0x%X, frame_id %lu, req_id %lu, timestamp 0x%lx, sof_status %d\n", event_data->session_hdl, event_data->u.frame_msg.link_hdl, event_data->u.frame_msg.frame_id, event_data->u.frame_msg.request_id, event_data->u.frame_msg.timestamp, event_data->u.frame_msg.sof_status);
}
if (event_data->session_hdl == s->road_cam.session_handle) {
handle_camera_event(&s->road_cam, event_data);

54
selfdrive/camerad/main.cc
Unescape View File

@ -1,48 +1,11 @@
#include <poll.h>
#include <sys/socket.h>
#include <unistd.h>
#include "selfdrive/camerad/cameras/camera_common.h"
#include <cassert>
#include <cstdio>
#include <thread>
#include "libyuv.h"
#include "cereal/visionipc/visionipc_server.h"
#include "selfdrive/common/clutil.h"
#include "selfdrive/common/params.h"
#include "selfdrive/common/swaglog.h"
#include "selfdrive/common/util.h"
#include "selfdrive/hardware/hw.h"
#ifdef QCOM
#include "selfdrive/camerad/cameras/camera_qcom.h"
#elif QCOM2
#include "selfdrive/camerad/cameras/camera_qcom2.h"
#elif WEBCAM
#include "selfdrive/camerad/cameras/camera_webcam.h"
#else
#include "selfdrive/camerad/cameras/camera_replay.h"
#endif
ExitHandler do_exit;
void party(cl_device_id device_id, cl_context context) {
MultiCameraState cameras = {};
VisionIpcServer vipc_server("camerad", device_id, context);
cameras_init(&vipc_server, &cameras, device_id, context);
cameras_open(&cameras);
vipc_server.start_listener();
cameras_run(&cameras);
}
#ifdef QCOM
#include "CL/cl_ext_qcom.h"
#endif
int main(int argc, char *argv[]) {
if (!Hardware::PC()) {
int ret;
@ -52,17 +15,6 @@ int main(int argc, char *argv[]) {
assert(ret == 0 || Params().getBool("IsOffroad")); // failure ok while offroad due to offlining cores
}
cl_device_id device_id = cl_get_device_id(CL_DEVICE_TYPE_DEFAULT);
// TODO: do this for QCOM2 too
#if defined(QCOM)
const cl_context_properties props[] = {CL_CONTEXT_PRIORITY_HINT_QCOM, CL_PRIORITY_HINT_HIGH_QCOM, 0};
cl_context context = CL_CHECK_ERR(clCreateContext(props, 1, &device_id, NULL, NULL, &err));
#else
cl_context context = CL_CHECK_ERR(clCreateContext(NULL, 1, &device_id, NULL, NULL, &err));
#endif
party(device_id, context);
CL_CHECK(clReleaseContext(context));
camerad_thread();
return 0;
}

5
selfdrive/camerad/test/ae_gray_test.cc
Unescape View File

@ -11,9 +11,10 @@
#include "selfdrive/camerad/cameras/camera_common.h"
// needed by camera_common.cc
ExitHandler do_exit;
void camera_autoexposure(CameraState *s, float grey_frac) {}
void cameras_init(VisionIpcServer *v, MultiCameraState *s, cl_device_id device_id, cl_context ctx) {}
void cameras_open(MultiCameraState *s) {}
void cameras_run(MultiCameraState *s) {}
int main() {
// set up fake camerabuf

2
selfdrive/car/car_helpers.py
Unescape View File

@ -125,7 +125,7 @@ def fingerprint(logcan, sendcan):
# The fingerprint dict is generated for all buses, this way the car interface
# can use it to detect a (valid) multipanda setup and initialize accordingly
if can.src < 128:
if can.src not in finger.keys():
if can.src not in finger:
finger[can.src] = {}
finger[can.src][can.address] = len(can.dat)

68
selfdrive/car/chrysler/carstate.py
Unescape View File

@ -75,33 +75,33 @@ class CarState(CarStateBase):
@staticmethod
def get_can_parser(CP):
signals = [
# sig_name, sig_address, default
("PRNDL", "GEAR", 0),
("DOOR_OPEN_FL", "DOORS", 0),
("DOOR_OPEN_FR", "DOORS", 0),
("DOOR_OPEN_RL", "DOORS", 0),
("DOOR_OPEN_RR", "DOORS", 0),
("BRAKE_PRESSED_2", "BRAKE_2", 0),
("ACCEL_134", "ACCEL_GAS_134", 0),
("SPEED_LEFT", "SPEED_1", 0),
("SPEED_RIGHT", "SPEED_1", 0),
("WHEEL_SPEED_FL", "WHEEL_SPEEDS", 0),
("WHEEL_SPEED_RR", "WHEEL_SPEEDS", 0),
("WHEEL_SPEED_RL", "WHEEL_SPEEDS", 0),
("WHEEL_SPEED_FR", "WHEEL_SPEEDS", 0),
("STEER_ANGLE", "STEERING", 0),
("STEERING_RATE", "STEERING", 0),
("TURN_SIGNALS", "STEERING_LEVERS", 0),
("ACC_STATUS_2", "ACC_2", 0),
("HIGH_BEAM_FLASH", "STEERING_LEVERS", 0),
("ACC_SPEED_CONFIG_KPH", "DASHBOARD", 0),
("CRUISE_STATE", "DASHBOARD", 0),
("TORQUE_DRIVER", "EPS_STATUS", 0),
("TORQUE_MOTOR", "EPS_STATUS", 0),
("LKAS_STATE", "EPS_STATUS", 1),
("COUNTER", "EPS_STATUS", -1),
("TRACTION_OFF", "TRACTION_BUTTON", 0),
("SEATBELT_DRIVER_UNLATCHED", "SEATBELT_STATUS", 0),
# sig_name, sig_address
("PRNDL", "GEAR"),
("DOOR_OPEN_FL", "DOORS"),
("DOOR_OPEN_FR", "DOORS"),
("DOOR_OPEN_RL", "DOORS"),
("DOOR_OPEN_RR", "DOORS"),
("BRAKE_PRESSED_2", "BRAKE_2"),
("ACCEL_134", "ACCEL_GAS_134"),
("SPEED_LEFT", "SPEED_1"),
("SPEED_RIGHT", "SPEED_1"),
("WHEEL_SPEED_FL", "WHEEL_SPEEDS"),
("WHEEL_SPEED_RR", "WHEEL_SPEEDS"),
("WHEEL_SPEED_RL", "WHEEL_SPEEDS"),
("WHEEL_SPEED_FR", "WHEEL_SPEEDS"),
("STEER_ANGLE", "STEERING"),
("STEERING_RATE", "STEERING"),
("TURN_SIGNALS", "STEERING_LEVERS"),
("ACC_STATUS_2", "ACC_2"),
("HIGH_BEAM_FLASH", "STEERING_LEVERS"),
("ACC_SPEED_CONFIG_KPH", "DASHBOARD"),
("CRUISE_STATE", "DASHBOARD"),
("TORQUE_DRIVER", "EPS_STATUS"),
("TORQUE_MOTOR", "EPS_STATUS"),
("LKAS_STATE", "EPS_STATUS"),
("COUNTER", "EPS_STATUS",),
("TRACTION_OFF", "TRACTION_BUTTON"),
("SEATBELT_DRIVER_UNLATCHED", "SEATBELT_STATUS"),
]
checks = [
@ -123,20 +123,20 @@ class CarState(CarStateBase):
if CP.enableBsm:
signals += [
("BLIND_SPOT_RIGHT", "BLIND_SPOT_WARNINGS", 0),
("BLIND_SPOT_LEFT", "BLIND_SPOT_WARNINGS", 0),
("BLIND_SPOT_RIGHT", "BLIND_SPOT_WARNINGS"),
("BLIND_SPOT_LEFT", "BLIND_SPOT_WARNINGS"),
]
checks += [("BLIND_SPOT_WARNINGS", 2)]
checks.append(("BLIND_SPOT_WARNINGS", 2))
return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 0)
@staticmethod
def get_cam_can_parser(CP):
signals = [
# sig_name, sig_address, default
("COUNTER", "LKAS_COMMAND", -1),
("CAR_MODEL", "LKAS_HUD", -1),
("LKAS_STATUS_OK", "LKAS_HEARTBIT", -1)
# sig_name, sig_address
("COUNTER", "LKAS_COMMAND"),
("CAR_MODEL", "LKAS_HUD"),
("LKAS_STATUS_OK", "LKAS_HEARTBIT")
]
checks = [
("LKAS_COMMAND", 100),

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

@ -60,8 +60,7 @@ class CarInterface(CarInterfaceBase):
ret.steeringRateLimited = self.CC.steer_rate_limited if self.CC is not None else False
# events
events = self.create_common_events(ret, extra_gears=[car.CarState.GearShifter.low],
gas_resume_speed=2.)
events = self.create_common_events(ret, extra_gears=[car.CarState.GearShifter.low])
if ret.vEgo < self.CP.minSteerSpeed:
events.add(car.CarEvent.EventName.belowSteerSpeed)

31
selfdrive/car/chrysler/radar_interface.py
Unescape View File

@ -11,30 +11,25 @@ NUMBER_MSGS = len(RADAR_MSGS_C) + len(RADAR_MSGS_D)
def _create_radar_can_parser(car_fingerprint):
msg_n = len(RADAR_MSGS_C)
# list of [(signal name, message name or number, initial values), (...)]
# [('RADAR_STATE', 1024, 0),
# ('LONG_DIST', 1072, 255),
# ('LONG_DIST', 1073, 255),
# ('LONG_DIST', 1074, 255),
# ('LONG_DIST', 1075, 255),
# list of [(signal name, message name or number), (...)]
# [('RADAR_STATE', 1024),
# ('LONG_DIST', 1072),
# ('LONG_DIST', 1073),
# ('LONG_DIST', 1074),
# ('LONG_DIST', 1075),
# The factor and offset are applied by the dbc parsing library, so the
# default values should be after the factor/offset are applied.
signals = list(zip(['LONG_DIST'] * msg_n +
['LAT_DIST'] * msg_n +
['REL_SPEED'] * msg_n,
RADAR_MSGS_C * 2 + # LONG_DIST, LAT_DIST
RADAR_MSGS_D, # REL_SPEED
[0] * msg_n + # LONG_DIST
[-1000] * msg_n + # LAT_DIST
[-146.278] * msg_n)) # REL_SPEED set to 0, factor/offset to this
['LAT_DIST'] * msg_n +
['REL_SPEED'] * msg_n,
RADAR_MSGS_C * 2 + # LONG_DIST, LAT_DIST
RADAR_MSGS_D)) # REL_SPEED
# TODO what are the checks actually used for?
# honda only checks the last message,
# toyota checks all the messages. Which do we want?
checks = list(zip(RADAR_MSGS_C +
RADAR_MSGS_D,
[20]*msg_n + # 20Hz (0.05s)
[20]*msg_n)) # 20Hz (0.05s)
RADAR_MSGS_D,
[20] * msg_n + # 20Hz (0.05s)
[20] * msg_n)) # 20Hz (0.05s)
return CANParser(DBC[car_fingerprint]['radar'], signals, checks, 1)

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

@ -1,5 +1,3 @@
# flake8: noqa
from selfdrive.car import dbc_dict
from cereal import car
Ecu = car.CarParams.Ecu

28
selfdrive/car/ford/carstate.py
Unescape View File

@ -39,20 +39,20 @@ class CarState(CarStateBase):
@staticmethod
def get_can_parser(CP):
signals = [
# sig_name, sig_address, default
("WhlRr_W_Meas", "WheelSpeed_CG1", 0.),
("WhlRl_W_Meas", "WheelSpeed_CG1", 0.),
("WhlFr_W_Meas", "WheelSpeed_CG1", 0.),
("WhlFl_W_Meas", "WheelSpeed_CG1", 0.),
("SteWhlRelInit_An_Sns", "Steering_Wheel_Data_CG1", 0.),
("Cruise_State", "Cruise_Status", 0.),
("Set_Speed", "Cruise_Status", 0.),
("LaActAvail_D_Actl", "Lane_Keep_Assist_Status", 0),
("LaHandsOff_B_Actl", "Lane_Keep_Assist_Status", 0),
("LaActDeny_B_Actl", "Lane_Keep_Assist_Status", 0),
("ApedPosScal_Pc_Actl", "EngineData_14", 0.),
("Dist_Incr", "Steering_Buttons", 0.),
("Brake_Drv_Appl", "Cruise_Status", 0.),
# sig_name, sig_address
("WhlRr_W_Meas", "WheelSpeed_CG1"),
("WhlRl_W_Meas", "WheelSpeed_CG1"),
("WhlFr_W_Meas", "WheelSpeed_CG1"),
("WhlFl_W_Meas", "WheelSpeed_CG1"),
("SteWhlRelInit_An_Sns", "Steering_Wheel_Data_CG1"),
("Cruise_State", "Cruise_Status"),
("Set_Speed", "Cruise_Status"),
("LaActAvail_D_Actl", "Lane_Keep_Assist_Status"),
("LaHandsOff_B_Actl", "Lane_Keep_Assist_Status"),
("LaActDeny_B_Actl", "Lane_Keep_Assist_Status"),
("ApedPosScal_Pc_Actl", "EngineData_14"),
("Dist_Incr", "Steering_Buttons"),
("Brake_Drv_Appl", "Cruise_Status"),
]
checks = []
return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 0, enforce_checks=False)

2
selfdrive/car/ford/interface.py
Unescape View File

@ -21,7 +21,7 @@ class CarInterface(CarInterfaceBase):
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.01], [0.005]] # TODO: tune this
ret.lateralTuning.pid.kf = 1. / MAX_ANGLE # MAX Steer angle to normalize FF
ret.steerActuatorDelay = 0.1 # Default delay, not measured yet
ret.steerLimitTimer = 0.8
ret.steerLimitTimer = 1.0
ret.steerRateCost = 1.0
ret.centerToFront = ret.wheelbase * 0.44
tire_stiffness_factor = 0.5328

6
selfdrive/car/ford/radar_interface.py
Unescape View File

@ -7,12 +7,12 @@ from selfdrive.car.interfaces import RadarInterfaceBase
RADAR_MSGS = list(range(0x500, 0x540))
def _create_radar_can_parser(car_fingerprint):
msg_n = len(RADAR_MSGS)
signals = list(zip(['X_Rel'] * msg_n + ['Angle'] * msg_n + ['V_Rel'] * msg_n,
RADAR_MSGS * 3,
[0] * msg_n + [0] * msg_n + [0] * msg_n))
checks = list(zip(RADAR_MSGS, [20]*msg_n))
RADAR_MSGS * 3))
checks = list(zip(RADAR_MSGS, [20] * msg_n))
return CANParser(DBC[car_fingerprint]['radar'], signals, checks, 1)

2
selfdrive/car/ford/values.py
Unescape View File

@ -1,5 +1,3 @@
# flake8: noqa
from selfdrive.car import dbc_dict
from cereal import car
Ecu = car.CarParams.Ecu

6
selfdrive/car/gm/carcontroller.py
Unescape View File

@ -27,7 +27,7 @@ class CarController():
self.packer_obj = CANPacker(DBC[CP.carFingerprint]['radar'])
self.packer_ch = CANPacker(DBC[CP.carFingerprint]['chassis'])
def update(self, enabled, CS, frame, actuators,
def update(self, c, enabled, CS, frame, actuators,
hud_v_cruise, hud_show_lanes, hud_show_car, hud_alert):
P = self.params
@ -41,7 +41,7 @@ class CarController():
if CS.lka_steering_cmd_counter != self.lka_steering_cmd_counter_last:
self.lka_steering_cmd_counter_last = CS.lka_steering_cmd_counter
elif (frame % P.STEER_STEP) == 0:
lkas_enabled = enabled and not (CS.out.steerWarning or CS.out.steerError) and CS.out.vEgo > P.MIN_STEER_SPEED
lkas_enabled = c.active and not (CS.out.steerWarning or CS.out.steerError) and CS.out.vEgo > P.MIN_STEER_SPEED
if lkas_enabled:
new_steer = int(round(actuators.steer * P.STEER_MAX))
apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, P)
@ -58,7 +58,7 @@ class CarController():
# Gas/regen and brakes - all at 25Hz
if (frame % 4) == 0:
if not enabled:
if not c.active:
# Stock ECU sends max regen when not enabled.
self.apply_gas = P.MAX_ACC_REGEN
self.apply_brake = 0

64
selfdrive/car/gm/carstate.py
Unescape View File

@ -3,8 +3,7 @@ from common.numpy_fast import mean
from opendbc.can.can_define import CANDefine
from opendbc.can.parser import CANParser
from selfdrive.car.interfaces import CarStateBase
from selfdrive.car.gm.values import DBC, CAR, AccState, CanBus, \
CruiseButtons, STEER_THRESHOLD
from selfdrive.car.gm.values import DBC, CAR, AccState, CanBus, STEER_THRESHOLD
class CarState(CarStateBase):
@ -79,33 +78,32 @@ class CarState(CarStateBase):
@staticmethod
def get_can_parser(CP):
# this function generates lists for signal, messages and initial values
signals = [
# sig_name, sig_address, default
("BrakePedalPosition", "EBCMBrakePedalPosition", 0),
("FrontLeftDoor", "BCMDoorBeltStatus", 0),
("FrontRightDoor", "BCMDoorBeltStatus", 0),
("RearLeftDoor", "BCMDoorBeltStatus", 0),
("RearRightDoor", "BCMDoorBeltStatus", 0),
("LeftSeatBelt", "BCMDoorBeltStatus", 0),
("RightSeatBelt", "BCMDoorBeltStatus", 0),
("TurnSignals", "BCMTurnSignals", 0),
("AcceleratorPedal2", "AcceleratorPedal2", 0),
("CruiseState", "AcceleratorPedal2", 0),
("ACCButtons", "ASCMSteeringButton", CruiseButtons.UNPRESS),
("SteeringWheelAngle", "PSCMSteeringAngle", 0),
("SteeringWheelRate", "PSCMSteeringAngle", 0),
("FLWheelSpd", "EBCMWheelSpdFront", 0),
("FRWheelSpd", "EBCMWheelSpdFront", 0),
("RLWheelSpd", "EBCMWheelSpdRear", 0),
("RRWheelSpd", "EBCMWheelSpdRear", 0),
("PRNDL", "ECMPRDNL", 0),
("LKADriverAppldTrq", "PSCMStatus", 0),
("LKATorqueDelivered", "PSCMStatus", 0),
("LKATorqueDeliveredStatus", "PSCMStatus", 0),
("TractionControlOn", "ESPStatus", 0),
("EPBClosed", "EPBStatus", 0),
("CruiseMainOn", "ECMEngineStatus", 0),
# sig_name, sig_address
("BrakePedalPosition", "EBCMBrakePedalPosition"),
("FrontLeftDoor", "BCMDoorBeltStatus"),
("FrontRightDoor", "BCMDoorBeltStatus"),
("RearLeftDoor", "BCMDoorBeltStatus"),
("RearRightDoor", "BCMDoorBeltStatus"),
("LeftSeatBelt", "BCMDoorBeltStatus"),
("RightSeatBelt", "BCMDoorBeltStatus"),
("TurnSignals", "BCMTurnSignals"),
("AcceleratorPedal2", "AcceleratorPedal2"),
("CruiseState", "AcceleratorPedal2"),
("ACCButtons", "ASCMSteeringButton"),
("SteeringWheelAngle", "PSCMSteeringAngle"),
("SteeringWheelRate", "PSCMSteeringAngle"),
("FLWheelSpd", "EBCMWheelSpdFront"),
("FRWheelSpd", "EBCMWheelSpdFront"),
("RLWheelSpd", "EBCMWheelSpdRear"),
("RRWheelSpd", "EBCMWheelSpdRear"),
("PRNDL", "ECMPRDNL"),
("LKADriverAppldTrq", "PSCMStatus"),
("LKATorqueDelivered", "PSCMStatus"),
("LKATorqueDeliveredStatus", "PSCMStatus"),
("TractionControlOn", "ESPStatus"),
("EPBClosed", "EPBStatus"),
("CruiseMainOn", "ECMEngineStatus"),
]
checks = [
@ -125,19 +123,15 @@ class CarState(CarStateBase):
]
if CP.carFingerprint == CAR.VOLT:
signals += [
("RegenPaddle", "EBCMRegenPaddle", 0),
]
checks += [
("EBCMRegenPaddle", 50),
]
signals.append(("RegenPaddle", "EBCMRegenPaddle"))
checks.append(("EBCMRegenPaddle", 50))
return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, CanBus.POWERTRAIN)
@staticmethod
def get_loopback_can_parser(CP):
signals = [
("RollingCounter", "ASCMLKASteeringCmd", 0),
("RollingCounter", "ASCMLKASteeringCmd"),
]
checks = [

4
selfdrive/car/gm/interface.py
Unescape View File

@ -47,7 +47,7 @@ class CarInterface(CarInterfaceBase):
# These cars have been put into dashcam only due to both a lack of users and test coverage.
# These cars likely still work fine. Once a user confirms each car works and a test route is
# added to selfdrive/test/test_routes, we can remove it from this list.
ret.dashcamOnly = candidate in {CAR.CADILLAC_ATS, CAR.HOLDEN_ASTRA, CAR.MALIBU}
ret.dashcamOnly = candidate in {CAR.CADILLAC_ATS, CAR.HOLDEN_ASTRA, CAR.MALIBU, CAR.BUICK_REGAL}
# Presence of a camera on the object bus is ok.
# Have to go to read_only if ASCM is online (ACC-enabled cars),
@ -226,7 +226,7 @@ class CarInterface(CarInterfaceBase):
# In GM, PCM faults out if ACC command overlaps user gas.
enabled = c.enabled and not self.CS.out.gasPressed
ret = self.CC.update(enabled, self.CS, self.frame,
ret = self.CC.update(c, enabled, self.CS, self.frame,
c.actuators,
hud_v_cruise, hud_control.lanesVisible,
hud_control.leadVisible, hud_control.visualAlert)

19
selfdrive/car/gm/radar_interface.py
Unescape View File

@ -14,6 +14,7 @@ NUM_SLOTS = 20
# messages that are present in DBC
LAST_RADAR_MSG = RADAR_HEADER_MSG + NUM_SLOTS
def create_radar_can_parser(car_fingerprint):
if car_fingerprint not in (CAR.VOLT, CAR.MALIBU, CAR.HOLDEN_ASTRA, CAR.ACADIA, CAR.CADILLAC_ATS, CAR.ESCALADE_ESV):
return None
@ -21,17 +22,13 @@ def create_radar_can_parser(car_fingerprint):
# C1A-ARS3-A by Continental
radar_targets = list(range(SLOT_1_MSG, SLOT_1_MSG + NUM_SLOTS))
signals = list(zip(['FLRRNumValidTargets',
'FLRRSnsrBlckd', 'FLRRYawRtPlsblityFlt',
'FLRRHWFltPrsntInt', 'FLRRAntTngFltPrsnt',
'FLRRAlgnFltPrsnt', 'FLRRSnstvFltPrsntInt'] +
['TrkRange'] * NUM_SLOTS + ['TrkRangeRate'] * NUM_SLOTS +
['TrkRangeAccel'] * NUM_SLOTS + ['TrkAzimuth'] * NUM_SLOTS +
['TrkWidth'] * NUM_SLOTS + ['TrkObjectID'] * NUM_SLOTS,
[RADAR_HEADER_MSG] * 7 + radar_targets * 6,
[0] * 7 +
[0.0] * NUM_SLOTS + [0.0] * NUM_SLOTS +
[0.0] * NUM_SLOTS + [0.0] * NUM_SLOTS +
[0.0] * NUM_SLOTS + [0] * NUM_SLOTS))
'FLRRSnsrBlckd', 'FLRRYawRtPlsblityFlt',
'FLRRHWFltPrsntInt', 'FLRRAntTngFltPrsnt',
'FLRRAlgnFltPrsnt', 'FLRRSnstvFltPrsntInt'] +
['TrkRange'] * NUM_SLOTS + ['TrkRangeRate'] * NUM_SLOTS +
['TrkRangeAccel'] * NUM_SLOTS + ['TrkAzimuth'] * NUM_SLOTS +
['TrkWidth'] * NUM_SLOTS + ['TrkObjectID'] * NUM_SLOTS,
[RADAR_HEADER_MSG] * 7 + radar_targets * 6))
checks = list({(s[1], 14) for s in signals})

16
selfdrive/car/gm/values.py
Unescape View File

@ -1,5 +1,3 @@
# flake8: noqa
from cereal import car
from selfdrive.car import dbc_dict
Ecu = car.CarParams.Ecu
@ -115,11 +113,11 @@ FINGERPRINTS = {
}
DBC = {
CAR.HOLDEN_ASTRA: dbc_dict('gm_global_a_powertrain', 'gm_global_a_object', chassis_dbc='gm_global_a_chassis'),
CAR.VOLT: dbc_dict('gm_global_a_powertrain', 'gm_global_a_object', chassis_dbc='gm_global_a_chassis'),
CAR.MALIBU: dbc_dict('gm_global_a_powertrain', 'gm_global_a_object', chassis_dbc='gm_global_a_chassis'),
CAR.ACADIA: dbc_dict('gm_global_a_powertrain', 'gm_global_a_object', chassis_dbc='gm_global_a_chassis'),
CAR.CADILLAC_ATS: dbc_dict('gm_global_a_powertrain', 'gm_global_a_object', chassis_dbc='gm_global_a_chassis'),
CAR.BUICK_REGAL: dbc_dict('gm_global_a_powertrain', 'gm_global_a_object', chassis_dbc='gm_global_a_chassis'),
CAR.ESCALADE_ESV: dbc_dict('gm_global_a_powertrain', 'gm_global_a_object', chassis_dbc='gm_global_a_chassis'),
CAR.HOLDEN_ASTRA: dbc_dict('gm_global_a_powertrain_generated', 'gm_global_a_object', chassis_dbc='gm_global_a_chassis'),
CAR.VOLT: dbc_dict('gm_global_a_powertrain_generated', 'gm_global_a_object', chassis_dbc='gm_global_a_chassis'),
CAR.MALIBU: dbc_dict('gm_global_a_powertrain_generated', 'gm_global_a_object', chassis_dbc='gm_global_a_chassis'),
CAR.ACADIA: dbc_dict('gm_global_a_powertrain_generated', 'gm_global_a_object', chassis_dbc='gm_global_a_chassis'),
CAR.CADILLAC_ATS: dbc_dict('gm_global_a_powertrain_generated', 'gm_global_a_object', chassis_dbc='gm_global_a_chassis'),
CAR.BUICK_REGAL: dbc_dict('gm_global_a_powertrain_generated', 'gm_global_a_object', chassis_dbc='gm_global_a_chassis'),
CAR.ESCALADE_ESV: dbc_dict('gm_global_a_powertrain_generated', 'gm_global_a_object', chassis_dbc='gm_global_a_chassis'),
}

4
selfdrive/car/honda/carcontroller.py
Unescape View File

@ -117,7 +117,7 @@ class CarController():
P = self.params
if enabled:
if active:
accel = actuators.accel
gas, brake = compute_gas_brake(actuators.accel, CS.out.vEgo, CS.CP.carFingerprint)
else:
@ -152,7 +152,7 @@ class CarController():
# steer torque is converted back to CAN reference (positive when steering right)
apply_steer = int(interp(-actuators.steer * P.STEER_MAX, P.STEER_LOOKUP_BP, P.STEER_LOOKUP_V))
lkas_active = enabled and not CS.steer_not_allowed
lkas_active = active and not CS.steer_not_allowed
# Send CAN commands.
can_sends = []

160
selfdrive/car/honda/carstate.py
Unescape View File

@ -11,34 +11,33 @@ TransmissionType = car.CarParams.TransmissionType
def get_can_signals(CP, gearbox_msg, main_on_sig_msg):
# this function generates lists for signal, messages and initial values
signals = [
("XMISSION_SPEED", "ENGINE_DATA", 0),
("WHEEL_SPEED_FL", "WHEEL_SPEEDS", 0),
("WHEEL_SPEED_FR", "WHEEL_SPEEDS", 0),
("WHEEL_SPEED_RL", "WHEEL_SPEEDS", 0),
("WHEEL_SPEED_RR", "WHEEL_SPEEDS", 0),
("STEER_ANGLE", "STEERING_SENSORS", 0),
("STEER_ANGLE_RATE", "STEERING_SENSORS", 0),
("MOTOR_TORQUE", "STEER_MOTOR_TORQUE", 0),
("STEER_TORQUE_SENSOR", "STEER_STATUS", 0),
("LEFT_BLINKER", "SCM_FEEDBACK", 0),
("RIGHT_BLINKER", "SCM_FEEDBACK", 0),
("GEAR", gearbox_msg, 0),
("SEATBELT_DRIVER_LAMP", "SEATBELT_STATUS", 1),
("SEATBELT_DRIVER_LATCHED", "SEATBELT_STATUS", 0),
("BRAKE_PRESSED", "POWERTRAIN_DATA", 0),
("BRAKE_SWITCH", "POWERTRAIN_DATA", 0),
("CRUISE_BUTTONS", "SCM_BUTTONS", 0),
("ESP_DISABLED", "VSA_STATUS", 1),
("USER_BRAKE", "VSA_STATUS", 0),
("BRAKE_HOLD_ACTIVE", "VSA_STATUS", 0),
("STEER_STATUS", "STEER_STATUS", 5),
("GEAR_SHIFTER", gearbox_msg, 0),
("PEDAL_GAS", "POWERTRAIN_DATA", 0),
("CRUISE_SETTING", "SCM_BUTTONS", 0),
("ACC_STATUS", "POWERTRAIN_DATA", 0),
("MAIN_ON", main_on_sig_msg, 0),
("XMISSION_SPEED", "ENGINE_DATA"),
("WHEEL_SPEED_FL", "WHEEL_SPEEDS"),
("WHEEL_SPEED_FR", "WHEEL_SPEEDS"),
("WHEEL_SPEED_RL", "WHEEL_SPEEDS"),
("WHEEL_SPEED_RR", "WHEEL_SPEEDS"),
("STEER_ANGLE", "STEERING_SENSORS"),
("STEER_ANGLE_RATE", "STEERING_SENSORS"),
("MOTOR_TORQUE", "STEER_MOTOR_TORQUE"),
("STEER_TORQUE_SENSOR", "STEER_STATUS"),
("LEFT_BLINKER", "SCM_FEEDBACK"),
("RIGHT_BLINKER", "SCM_FEEDBACK"),
("GEAR", gearbox_msg),
("SEATBELT_DRIVER_LAMP", "SEATBELT_STATUS"),
("SEATBELT_DRIVER_LATCHED", "SEATBELT_STATUS"),
("BRAKE_PRESSED", "POWERTRAIN_DATA"),
("BRAKE_SWITCH", "POWERTRAIN_DATA"),
("CRUISE_BUTTONS", "SCM_BUTTONS"),
("ESP_DISABLED", "VSA_STATUS"),
("USER_BRAKE", "VSA_STATUS"),
("BRAKE_HOLD_ACTIVE", "VSA_STATUS"),
("STEER_STATUS", "STEER_STATUS"),
("GEAR_SHIFTER", gearbox_msg),
("PEDAL_GAS", "POWERTRAIN_DATA"),
("CRUISE_SETTING", "SCM_BUTTONS"),
("ACC_STATUS", "POWERTRAIN_DATA"),
("MAIN_ON", main_on_sig_msg),
]
checks = [
@ -65,22 +64,18 @@ def get_can_signals(CP, gearbox_msg, main_on_sig_msg):
]
if CP.carFingerprint in (CAR.CRV_HYBRID, CAR.CIVIC_BOSCH_DIESEL, CAR.ACURA_RDX_3G, CAR.HONDA_E):
checks += [
(gearbox_msg, 50),
]
checks.append((gearbox_msg, 50))
else:
checks += [
(gearbox_msg, 100),
]
checks.append((gearbox_msg, 100))
if CP.carFingerprint in HONDA_BOSCH_ALT_BRAKE_SIGNAL:
signals += [("BRAKE_PRESSED", "BRAKE_MODULE", 0)]
checks += [("BRAKE_MODULE", 50)]
signals.append(("BRAKE_PRESSED", "BRAKE_MODULE"))
checks.append(("BRAKE_MODULE", 50))
if CP.carFingerprint in HONDA_BOSCH:
signals += [
("EPB_STATE", "EPB_STATUS", 0),
("IMPERIAL_UNIT", "CAR_SPEED", 1),
("EPB_STATE", "EPB_STATUS"),
("IMPERIAL_UNIT", "CAR_SPEED"),
]
checks += [
("EPB_STATUS", 50),
@ -89,65 +84,65 @@ def get_can_signals(CP, gearbox_msg, main_on_sig_msg):
if not CP.openpilotLongitudinalControl:
signals += [
("CRUISE_CONTROL_LABEL", "ACC_HUD", 0),
("CRUISE_SPEED", "ACC_HUD", 0),
("ACCEL_COMMAND", "ACC_CONTROL", 0),
("AEB_STATUS", "ACC_CONTROL", 0),
("CRUISE_CONTROL_LABEL", "ACC_HUD"),
("CRUISE_SPEED", "ACC_HUD"),
("ACCEL_COMMAND", "ACC_CONTROL"),
("AEB_STATUS", "ACC_CONTROL"),
]
checks += [
("ACC_HUD", 10),
("ACC_CONTROL", 50),
]
else: # Nidec signals
signals += [("CRUISE_SPEED_PCM", "CRUISE", 0),
("CRUISE_SPEED_OFFSET", "CRUISE_PARAMS", 0)]
signals += [("CRUISE_SPEED_PCM", "CRUISE"),
("CRUISE_SPEED_OFFSET", "CRUISE_PARAMS")]
if CP.carFingerprint == CAR.ODYSSEY_CHN:
checks += [("CRUISE_PARAMS", 10)]
checks.append(("CRUISE_PARAMS", 10))
else:
checks += [("CRUISE_PARAMS", 50)]
checks.append(("CRUISE_PARAMS", 50))
if CP.carFingerprint in (CAR.ACCORD, CAR.ACCORDH, CAR.CIVIC_BOSCH, CAR.CIVIC_BOSCH_DIESEL, CAR.CRV_HYBRID, CAR.INSIGHT, CAR.ACURA_RDX_3G, CAR.HONDA_E):
signals += [("DRIVERS_DOOR_OPEN", "SCM_FEEDBACK", 1)]
signals.append(("DRIVERS_DOOR_OPEN", "SCM_FEEDBACK"))
elif CP.carFingerprint == CAR.ODYSSEY_CHN:
signals += [("DRIVERS_DOOR_OPEN", "SCM_BUTTONS", 1)]
signals.append(("DRIVERS_DOOR_OPEN", "SCM_BUTTONS"))
elif CP.carFingerprint in (CAR.FREED, CAR.HRV):
signals += [("DRIVERS_DOOR_OPEN", "SCM_BUTTONS", 1),
("WHEELS_MOVING", "STANDSTILL", 1)]
signals += [("DRIVERS_DOOR_OPEN", "SCM_BUTTONS"),
("WHEELS_MOVING", "STANDSTILL")]
else:
signals += [("DOOR_OPEN_FL", "DOORS_STATUS", 1),
("DOOR_OPEN_FR", "DOORS_STATUS", 1),
("DOOR_OPEN_RL", "DOORS_STATUS", 1),
("DOOR_OPEN_RR", "DOORS_STATUS", 1),
("WHEELS_MOVING", "STANDSTILL", 1)]
signals += [("DOOR_OPEN_FL", "DOORS_STATUS"),
("DOOR_OPEN_FR", "DOORS_STATUS"),
("DOOR_OPEN_RL", "DOORS_STATUS"),
("DOOR_OPEN_RR", "DOORS_STATUS"),
("WHEELS_MOVING", "STANDSTILL")]
checks += [
("DOORS_STATUS", 3),
("STANDSTILL", 50),
]
if CP.carFingerprint == CAR.CIVIC:
signals += [("IMPERIAL_UNIT", "HUD_SETTING", 0),
("EPB_STATE", "EPB_STATUS", 0)]
signals += [("IMPERIAL_UNIT", "HUD_SETTING"),
("EPB_STATE", "EPB_STATUS")]
checks += [
("HUD_SETTING", 50),
("EPB_STATUS", 50),
]
elif CP.carFingerprint in (CAR.ODYSSEY, CAR.ODYSSEY_CHN):
signals += [("EPB_STATE", "EPB_STATUS", 0)]
checks += [("EPB_STATUS", 50)]
signals.append(("EPB_STATE", "EPB_STATUS"))
checks.append(("EPB_STATUS", 50))
# add gas interceptor reading if we are using it
if CP.enableGasInterceptor:
signals.append(("INTERCEPTOR_GAS", "GAS_SENSOR", 0))
signals.append(("INTERCEPTOR_GAS2", "GAS_SENSOR", 0))
signals.append(("INTERCEPTOR_GAS", "GAS_SENSOR"))
signals.append(("INTERCEPTOR_GAS2", "GAS_SENSOR"))
checks.append(("GAS_SENSOR", 50))
if CP.openpilotLongitudinalControl:
signals += [
("BRAKE_ERROR_1", "STANDSTILL", 1),
("BRAKE_ERROR_2", "STANDSTILL", 1)
("BRAKE_ERROR_1", "STANDSTILL"),
("BRAKE_ERROR_2", "STANDSTILL")
]
checks += [("STANDSTILL", 50)]
checks.append(("STANDSTILL", 50))
return signals, checks
@ -167,6 +162,7 @@ class CarState(CarStateBase):
self.shifter_values = can_define.dv[self.gearbox_msg]["GEAR_SHIFTER"]
self.steer_status_values = defaultdict(lambda: "UNKNOWN", can_define.dv["STEER_STATUS"]["STEER_STATUS"])
self.brake_error = False
self.brake_switch_prev = 0
self.brake_switch_prev_ts = 0
self.cruise_setting = 0
@ -207,9 +203,7 @@ class CarState(CarStateBase):
# LOW_SPEED_LOCKOUT is not worth a warning
ret.steerWarning = steer_status not in ("NORMAL", "LOW_SPEED_LOCKOUT", "NO_TORQUE_ALERT_2")
if not self.CP.openpilotLongitudinalControl:
self.brake_error = 0
else:
if self.CP.openpilotLongitudinalControl:
self.brake_error = cp.vl["STANDSTILL"]["BRAKE_ERROR_1"] or cp.vl["STANDSTILL"]["BRAKE_ERROR_2"]
ret.espDisabled = cp.vl["VSA_STATUS"]["ESP_DISABLED"] != 0
@ -245,15 +239,11 @@ class CarState(CarStateBase):
gear = int(cp.vl[self.gearbox_msg]["GEAR_SHIFTER"])
ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(gear, None))
ret.gas = cp.vl["POWERTRAIN_DATA"]["PEDAL_GAS"]
# this is a hack for the interceptor. This is now only used in the simulation
# TODO: Replace tests by toyota so this can go away
if self.CP.enableGasInterceptor:
user_gas = (cp.vl["GAS_SENSOR"]["INTERCEPTOR_GAS"] + cp.vl["GAS_SENSOR"]["INTERCEPTOR_GAS2"]) / 2.
ret.gasPressed = user_gas > 1e-5 # this works because interceptor reads < 0 when pedal position is 0. Once calibrated, this will change
ret.gas = (cp.vl["GAS_SENSOR"]["INTERCEPTOR_GAS"] + cp.vl["GAS_SENSOR"]["INTERCEPTOR_GAS2"]) / 2.
else:
ret.gasPressed = ret.gas > 1e-5
ret.gas = cp.vl["POWERTRAIN_DATA"]["PEDAL_GAS"]
ret.gasPressed = ret.gas > 1e-5
ret.steeringTorque = cp.vl["STEER_STATUS"]["STEER_TORQUE_SENSOR"]
ret.steeringTorqueEps = cp.vl["STEER_MOTOR_TORQUE"]["MOTOR_TORQUE"]
@ -288,8 +278,8 @@ class CarState(CarStateBase):
ret.cruiseState.available = bool(cp.vl[self.main_on_sig_msg]["MAIN_ON"])
# Gets rid of Pedal Grinding noise when brake is pressed at slow speeds for some models
if self.CP.carFingerprint in (CAR.PILOT, CAR.PILOT_2019, CAR.PASSPORT, CAR.RIDGELINE):
if ret.brake > 0.05:
if self.CP.carFingerprint in (CAR.PILOT, CAR.PASSPORT, CAR.RIDGELINE):
if ret.brake > 0.1:
ret.brakePressed = True
# TODO: discover the CAN msg that has the imperial unit bit for all other cars
@ -334,14 +324,14 @@ class CarState(CarStateBase):
]
if CP.carFingerprint not in HONDA_BOSCH:
signals += [("COMPUTER_BRAKE", "BRAKE_COMMAND", 0),
("AEB_REQ_1", "BRAKE_COMMAND", 0),
("FCW", "BRAKE_COMMAND", 0),
("CHIME", "BRAKE_COMMAND", 0),
("FCM_OFF", "ACC_HUD", 0),
("FCM_OFF_2", "ACC_HUD", 0),
("FCM_PROBLEM", "ACC_HUD", 0),
("ICONS", "ACC_HUD", 0)]
signals += [("COMPUTER_BRAKE", "BRAKE_COMMAND"),
("AEB_REQ_1", "BRAKE_COMMAND"),
("FCW", "BRAKE_COMMAND"),
("CHIME", "BRAKE_COMMAND"),
("FCM_OFF", "ACC_HUD"),
("FCM_OFF_2", "ACC_HUD"),
("FCM_PROBLEM", "ACC_HUD"),
("ICONS", "ACC_HUD")]
checks += [
("ACC_HUD", 10),
("BRAKE_COMMAND", 50),
@ -352,8 +342,8 @@ class CarState(CarStateBase):
@staticmethod
def get_body_can_parser(CP):
if CP.enableBsm and CP.carFingerprint == CAR.CRV_5G:
signals = [("BSM_ALERT", "BSM_STATUS_RIGHT", 0),
("BSM_ALERT", "BSM_STATUS_LEFT", 0)]
signals = [("BSM_ALERT", "BSM_STATUS_RIGHT"),
("BSM_ALERT", "BSM_STATUS_LEFT")]
checks = [
("BSM_STATUS_LEFT", 3),

13
selfdrive/car/honda/interface.py
Unescape View File

@ -47,7 +47,6 @@ class CarInterface(CarInterfaceBase):
ret.openpilotLongitudinalControl = True
ret.pcmCruise = not ret.enableGasInterceptor
ret.communityFeature = ret.enableGasInterceptor
if candidate == CAR.CRV_5G:
ret.enableBsm = 0x12f8bfa7 in fingerprint[0]
@ -250,17 +249,7 @@ class CarInterface(CarInterfaceBase):
tire_stiffness_factor = 0.82
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.28], [0.08]]
elif candidate in (CAR.PILOT, CAR.PILOT_2019):
stop_and_go = False
ret.mass = 4204. * CV.LB_TO_KG + STD_CARGO_KG # average weight
ret.wheelbase = 2.82
ret.centerToFront = ret.wheelbase * 0.428
ret.steerRatio = 17.25 # as spec
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end
tire_stiffness_factor = 0.444
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.38], [0.11]]
elif candidate == CAR.PASSPORT:
elif candidate in (CAR.PILOT, CAR.PASSPORT):
stop_and_go = False
ret.mass = 4204. * CV.LB_TO_KG + STD_CARGO_KG # average weight
ret.wheelbase = 2.82

8
selfdrive/car/honda/radar_interface.py
Unescape View File

@ -4,13 +4,13 @@ from opendbc.can.parser import CANParser
from selfdrive.car.interfaces import RadarInterfaceBase
from selfdrive.car.honda.values import DBC
def _create_nidec_can_parser(car_fingerprint):
radar_messages = [0x400] + list(range(0x430, 0x43A)) + list(range(0x440, 0x446))
signals = list(zip(['RADAR_STATE'] +
['LONG_DIST'] * 16 + ['NEW_TRACK'] * 16 + ['LAT_DIST'] * 16 +
['REL_SPEED'] * 16,
[0x400] + radar_messages[1:] * 4,
[0] + [255] * 16 + [1] * 16 + [0] * 16 + [0] * 16))
['LONG_DIST'] * 16 + ['NEW_TRACK'] * 16 + ['LAT_DIST'] * 16 +
['REL_SPEED'] * 16,
[0x400] + radar_messages[1:] * 4))
checks = [(s[1], 20) for s in signals]
return CANParser(DBC[car_fingerprint]['radar'], signals, checks, 1)

84
selfdrive/car/honda/values.py
Unescape View File

@ -82,7 +82,6 @@ class CAR:
ACURA_RDX = "ACURA RDX 2018"
ACURA_RDX_3G = "ACURA RDX 2020"
PILOT = "HONDA PILOT 2017"
PILOT_2019 = "HONDA PILOT 2019"
PASSPORT = "HONDA PASSPORT 2021"
RIDGELINE = "HONDA RIDGELINE 2017"
INSIGHT = "HONDA INSIGHT 2019"
@ -990,74 +989,51 @@ FW_VERSIONS = {
b'37805-RLV-C530\x00\x00',
b'37805-RLV-C910\x00\x00',
],
(Ecu.gateway, 0x18daeff1, None): [
b'38897-TG7-A030\x00\x00',
b'38897-TG7-A040\x00\x00',
b'38897-TG7-A110\x00\x00',
b'38897-TG7-A210\x00\x00',
],
(Ecu.eps, 0x18da30f1, None): [
b'39990-TG7-A030\x00\x00',
b'39990-TG7-A040\x00\x00',
b'39990-TG7-A060\x00\x00',
],
(Ecu.fwdCamera, 0x18dab0f1, None): [
b'36161-TG7-A520\x00\x00',
b'36161-TG7-A720\x00\x00',
b'36161-TG7-A820\x00\x00',
b'36161-TG7-C520\x00\x00',
b'36161-TG7-D520\x00\x00',
b'36161-TG8-A520\x00\x00',
b'36161-TG8-A720\x00\x00',
],
(Ecu.srs, 0x18da53f1, None): [
b'77959-TG7-A110\x00\x00',
b'77959-TG7-A020\x00\x00',
],
(Ecu.combinationMeter, 0x18da60f1, None): [
b'78109-TG7-A040\x00\x00',
b'78109-TG7-A050\x00\x00',
b'78109-TG7-A420\x00\x00',
b'78109-TG7-A520\x00\x00',
b'78109-TG7-A720\x00\x00',
b'78109-TG7-D020\x00\x00',
b'78109-TG8-A420\x00\x00',
b'78109-TG8-A520\x00\x00',
],
(Ecu.vsa, 0x18da28f1, None): [
b'57114-TG7-A130\x00\x00',
b'57114-TG7-A140\x00\x00',
b'57114-TG7-A230\x00\x00',
b'57114-TG7-A240\x00\x00',
b'57114-TG8-A140\x00\x00',
b'57114-TG8-A240\x00\x00',
],
},
CAR.PILOT_2019: {
(Ecu.eps, 0x18da30f1, None): [
b'39990-TG7-A060\x00\x00',
b'39990-TG7-A070\x00\x00',
b'39990-TGS-A230\x00\x00',
],
(Ecu.gateway, 0x18daeff1, None): [
b'38897-TG7-A030\x00\x00',
b'38897-TG7-A040\x00\x00',
b'38897-TG7-A110\x00\x00',
b'38897-TG7-A210\x00\x00',
],
(Ecu.fwdCamera, 0x18dab0f1, None): [
b'36161-TG7-A310\x00\x00',
b'36161-TG7-A520\x00\x00',
b'36161-TG7-A630\x00\x00',
b'36161-TG7-A720\x00\x00',
b'36161-TG7-A820\x00\x00',
b'36161-TG7-A930\x00\x00',
b'36161-TG7-C520\x00\x00',
b'36161-TG7-D520\x00\x00',
b'36161-TG7-D630\x00\x00',
b'36161-TG7-Y630\x00\x00',
b'36161-TG8-A520\x00\x00',
b'36161-TG8-A630\x00\x00',
b'36161-TG8-A720\x00\x00',
b'36161-TG8-A830\x00\x00',
b'36161-TGS-A130\x00\x00',
b'36161-TGT-A030\x00\x00',
b'36161-TGT-A130\x00\x00',
],
(Ecu.srs, 0x18da53f1, None): [
b'77959-TG7-A020\x00\x00',
b'77959-TG7-A110\x00\x00',
b'77959-TG7-A210\x00\x00',
b'77959-TG7-Y210\x00\x00',
b'77959-TGS-A010\x00\x00',
],
(Ecu.combinationMeter, 0x18da60f1, None): [
b'78109-TG7-A040\x00\x00',
b'78109-TG7-A050\x00\x00',
b'78109-TG7-A420\x00\x00',
b'78109-TG7-A520\x00\x00',
b'78109-TG7-A720\x00\x00',
b'78109-TG7-AJ10\x00\x00',
b'78109-TG7-AJ20\x00\x00',
b'78109-TG7-AK10\x00\x00',
@ -1069,8 +1045,11 @@ FW_VERSIONS = {
b'78109-TG7-AT20\x00\x00',
b'78109-TG7-AU20\x00\x00',
b'78109-TG7-AX20\x00\x00',
b'78109-TG7-D020\x00\x00',
b'78109-TG7-DJ10\x00\x00',
b'78109-TG7-YK20\x00\x00',
b'78109-TG8-A420\x00\x00',
b'78109-TG8-A520\x00\x00',
b'78109-TG8-AJ10\x00\x00',
b'78109-TG8-AJ20\x00\x00',
b'78109-TG8-AK20\x00\x00',
@ -1080,8 +1059,14 @@ FW_VERSIONS = {
b'78109-TGT-AK30\x00\x00',
],
(Ecu.vsa, 0x18da28f1, None): [
b'57114-TG7-A130\x00\x00',
b'57114-TG7-A140\x00\x00',
b'57114-TG7-A230\x00\x00',
b'57114-TG7-A240\x00\x00',
b'57114-TG7-A630\x00\x00',
b'57114-TG7-A730\x00\x00',
b'57114-TG8-A140\x00\x00',
b'57114-TG8-A240\x00\x00',
b'57114-TG8-A630\x00\x00',
b'57114-TG8-A730\x00\x00',
b'57114-TGS-A530\x00\x00',
@ -1371,10 +1356,9 @@ DBC = {
CAR.HRV: dbc_dict('honda_fit_ex_2018_can_generated', 'acura_ilx_2016_nidec'),
CAR.ODYSSEY: dbc_dict('honda_odyssey_exl_2018_generated', 'acura_ilx_2016_nidec'),
CAR.ODYSSEY_CHN: dbc_dict('honda_odyssey_extreme_edition_2018_china_can_generated', 'acura_ilx_2016_nidec'),
CAR.PILOT: dbc_dict('honda_pilot_touring_2017_can_generated', 'acura_ilx_2016_nidec'),
CAR.PILOT_2019: dbc_dict('honda_pilot_touring_2017_can_generated', 'acura_ilx_2016_nidec'),
CAR.PASSPORT: dbc_dict('honda_pilot_touring_2017_can_generated', 'acura_ilx_2016_nidec'),
CAR.RIDGELINE: dbc_dict('honda_ridgeline_black_edition_2017_can_generated', 'acura_ilx_2016_nidec'),
CAR.PILOT: dbc_dict('acura_ilx_2016_can_generated', 'acura_ilx_2016_nidec'),
CAR.PASSPORT: dbc_dict('acura_ilx_2016_can_generated', 'acura_ilx_2016_nidec'),
CAR.RIDGELINE: dbc_dict('acura_ilx_2016_can_generated', 'acura_ilx_2016_nidec'),
CAR.INSIGHT: dbc_dict('honda_insight_ex_2019_can_generated', None),
CAR.HONDA_E: dbc_dict('acura_rdx_2020_can_generated', None),
}
@ -1387,7 +1371,7 @@ STEER_THRESHOLD = {
HONDA_NIDEC_ALT_PCM_ACCEL = {CAR.ODYSSEY}
HONDA_NIDEC_ALT_SCM_MESSAGES = {CAR.ACURA_ILX, CAR.ACURA_RDX, CAR.CRV, CAR.CRV_EU, CAR.FIT, CAR.FREED, CAR.HRV, CAR.ODYSSEY_CHN,
CAR.PILOT, CAR.PILOT_2019, CAR.PASSPORT, CAR.RIDGELINE}
CAR.PILOT, CAR.PASSPORT, CAR.RIDGELINE}
HONDA_BOSCH = {CAR.ACCORD, CAR.ACCORDH, CAR.CIVIC_BOSCH, CAR.CIVIC_BOSCH_DIESEL, CAR.CRV_5G,
CAR.CRV_HYBRID, CAR.INSIGHT, CAR.ACURA_RDX_3G, CAR.HONDA_E}
CAR.CRV_HYBRID, CAR.INSIGHT, CAR.ACURA_RDX_3G, CAR.HONDA_E}
HONDA_BOSCH_ALT_BRAKE_SIGNAL = {CAR.ACCORD, CAR.CRV_5G, CAR.ACURA_RDX_3G}

6
selfdrive/car/hyundai/carcontroller.py
Unescape View File

@ -46,7 +46,7 @@ class CarController():
self.last_resume_frame = 0
self.accel = 0
def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, visual_alert, hud_speed,
def update(self, c, enabled, CS, frame, actuators, pcm_cancel_cmd, visual_alert, hud_speed,
left_lane, right_lane, left_lane_depart, right_lane_depart):
# Steering Torque
new_steer = int(round(actuators.steer * self.p.STEER_MAX))
@ -54,7 +54,7 @@ class CarController():
self.steer_rate_limited = new_steer != apply_steer
# disable when temp fault is active, or below LKA minimum speed
lkas_active = enabled and not CS.out.steerWarning and CS.out.vEgo >= CS.CP.minSteerSpeed
lkas_active = c.active and not CS.out.steerWarning and CS.out.vEgo >= CS.CP.minSteerSpeed
if not lkas_active:
apply_steer = 0
@ -89,7 +89,7 @@ class CarController():
if frame % 2 == 0 and CS.CP.openpilotLongitudinalControl:
lead_visible = False
accel = actuators.accel if enabled else 0
accel = actuators.accel if c.active else 0
jerk = clip(2.0 * (accel - CS.out.aEgo), -12.7, 12.7)

205
selfdrive/car/hyundai/carstate.py
Unescape View File

@ -120,57 +120,57 @@ class CarState(CarStateBase):
@staticmethod
def get_can_parser(CP):
signals = [
# sig_name, sig_address, default
("WHL_SPD_FL", "WHL_SPD11", 0),
("WHL_SPD_FR", "WHL_SPD11", 0),
("WHL_SPD_RL", "WHL_SPD11", 0),
("WHL_SPD_RR", "WHL_SPD11", 0),
("YAW_RATE", "ESP12", 0),
("CF_Gway_DrvSeatBeltInd", "CGW4", 1),
("CF_Gway_DrvSeatBeltSw", "CGW1", 0),
("CF_Gway_DrvDrSw", "CGW1", 0), # Driver Door
("CF_Gway_AstDrSw", "CGW1", 0), # Passenger door
("CF_Gway_RLDrSw", "CGW2", 0), # Rear reft door
("CF_Gway_RRDrSw", "CGW2", 0), # Rear right door
("CF_Gway_TurnSigLh", "CGW1", 0),
("CF_Gway_TurnSigRh", "CGW1", 0),
("CF_Gway_ParkBrakeSw", "CGW1", 0),
("CYL_PRES", "ESP12", 0),
("CF_Clu_CruiseSwState", "CLU11", 0),
("CF_Clu_CruiseSwMain", "CLU11", 0),
("CF_Clu_SldMainSW", "CLU11", 0),
("CF_Clu_ParityBit1", "CLU11", 0),
("CF_Clu_VanzDecimal" , "CLU11", 0),
("CF_Clu_Vanz", "CLU11", 0),
("CF_Clu_SPEED_UNIT", "CLU11", 0),
("CF_Clu_DetentOut", "CLU11", 0),
("CF_Clu_RheostatLevel", "CLU11", 0),
("CF_Clu_CluInfo", "CLU11", 0),
("CF_Clu_AmpInfo", "CLU11", 0),
("CF_Clu_AliveCnt1", "CLU11", 0),
("ACCEnable", "TCS13", 0),
("ACC_REQ", "TCS13", 0),
("DriverBraking", "TCS13", 0),
("StandStill", "TCS13", 0),
("PBRAKE_ACT", "TCS13", 0),
("ESC_Off_Step", "TCS15", 0),
("AVH_LAMP", "TCS15", 0),
("CR_Mdps_StrColTq", "MDPS12", 0),
("CF_Mdps_ToiActive", "MDPS12", 0),
("CF_Mdps_ToiUnavail", "MDPS12", 0),
("CF_Mdps_ToiFlt", "MDPS12", 0),
("CR_Mdps_OutTq", "MDPS12", 0),
("SAS_Angle", "SAS11", 0),
("SAS_Speed", "SAS11", 0),
# sig_name, sig_address
("WHL_SPD_FL", "WHL_SPD11"),
("WHL_SPD_FR", "WHL_SPD11"),
("WHL_SPD_RL", "WHL_SPD11"),
("WHL_SPD_RR", "WHL_SPD11"),
("YAW_RATE", "ESP12"),
("CF_Gway_DrvSeatBeltInd", "CGW4"),
("CF_Gway_DrvSeatBeltSw", "CGW1"),
("CF_Gway_DrvDrSw", "CGW1"), # Driver Door
("CF_Gway_AstDrSw", "CGW1"), # Passenger door
("CF_Gway_RLDrSw", "CGW2"), # Rear reft door
("CF_Gway_RRDrSw", "CGW2"), # Rear right door
("CF_Gway_TurnSigLh", "CGW1"),
("CF_Gway_TurnSigRh", "CGW1"),
("CF_Gway_ParkBrakeSw", "CGW1"),
("CYL_PRES", "ESP12"),
("CF_Clu_CruiseSwState", "CLU11"),
("CF_Clu_CruiseSwMain", "CLU11"),
("CF_Clu_SldMainSW", "CLU11"),
("CF_Clu_ParityBit1", "CLU11"),
("CF_Clu_VanzDecimal" , "CLU11"),
("CF_Clu_Vanz", "CLU11"),
("CF_Clu_SPEED_UNIT", "CLU11"),
("CF_Clu_DetentOut", "CLU11"),
("CF_Clu_RheostatLevel", "CLU11"),
("CF_Clu_CluInfo", "CLU11"),
("CF_Clu_AmpInfo", "CLU11"),
("CF_Clu_AliveCnt1", "CLU11"),
("ACCEnable", "TCS13"),
("ACC_REQ", "TCS13"),
("DriverBraking", "TCS13"),
("StandStill", "TCS13"),
("PBRAKE_ACT", "TCS13"),
("ESC_Off_Step", "TCS15"),
("AVH_LAMP", "TCS15"),
("CR_Mdps_StrColTq", "MDPS12"),
("CF_Mdps_ToiActive", "MDPS12"),
("CF_Mdps_ToiUnavail", "MDPS12"),
("CF_Mdps_ToiFlt", "MDPS12"),
("CR_Mdps_OutTq", "MDPS12"),
("SAS_Angle", "SAS11"),
("SAS_Speed", "SAS11"),
]
checks = [
@ -189,11 +189,11 @@ class CarState(CarStateBase):
if not CP.openpilotLongitudinalControl:
signals += [
("MainMode_ACC", "SCC11", 0),
("VSetDis", "SCC11", 0),
("SCCInfoDisplay", "SCC11", 0),
("ACC_ObjDist", "SCC11", 0),
("ACCMode", "SCC12", 1),
("MainMode_ACC", "SCC11"),
("VSetDis", "SCC11"),
("SCCInfoDisplay", "SCC11"),
("ACC_ObjDist", "SCC11"),
("ACCMode", "SCC12"),
]
checks += [
@ -203,39 +203,33 @@ class CarState(CarStateBase):
if CP.carFingerprint in FEATURES["use_fca"]:
signals += [
("FCA_CmdAct", "FCA11", 0),
("CF_VSM_Warn", "FCA11", 0),
("FCA_CmdAct", "FCA11"),
("CF_VSM_Warn", "FCA11"),
]
checks += [("FCA11", 50)]
checks.append(("FCA11", 50))
else:
signals += [
("AEB_CmdAct", "SCC12", 0),
("CF_VSM_Warn", "SCC12", 0),
("AEB_CmdAct", "SCC12"),
("CF_VSM_Warn", "SCC12"),
]
if CP.enableBsm:
signals += [
("CF_Lca_IndLeft", "LCA11", 0),
("CF_Lca_IndRight", "LCA11", 0),
("CF_Lca_IndLeft", "LCA11"),
("CF_Lca_IndRight", "LCA11"),
]
checks += [("LCA11", 50)]
checks.append(("LCA11", 50))
if CP.carFingerprint in (HYBRID_CAR | EV_CAR):
if CP.carFingerprint in HYBRID_CAR:
signals += [
("CR_Vcu_AccPedDep_Pos", "E_EMS11", 0)
]
signals.append(("CR_Vcu_AccPedDep_Pos", "E_EMS11"))
else:
signals += [
("Accel_Pedal_Pos", "E_EMS11", 0)
]
checks += [
("E_EMS11", 50),
]
signals.append(("Accel_Pedal_Pos", "E_EMS11"))
checks.append(("E_EMS11", 50))
else:
signals += [
("PV_AV_CAN", "EMS12", 0),
("CF_Ems_AclAct", "EMS16", 0),
("PV_AV_CAN", "EMS12"),
("CF_Ems_AclAct", "EMS16"),
]
checks += [
("EMS12", 100),
@ -243,52 +237,39 @@ class CarState(CarStateBase):
]
if CP.carFingerprint in FEATURES["use_cluster_gears"]:
signals += [
("CF_Clu_Gear", "CLU15", 0),
]
checks += [
("CLU15", 5)
]
signals.append(("CF_Clu_Gear", "CLU15"))
checks.append(("CLU15", 5))
elif CP.carFingerprint in FEATURES["use_tcu_gears"]:
signals += [
("CUR_GR", "TCU12", 0)
]
checks += [
("TCU12", 100)
]
signals.append(("CUR_GR", "TCU12"))
checks.append(("TCU12", 100))
elif CP.carFingerprint in FEATURES["use_elect_gears"]:
signals += [("Elect_Gear_Shifter", "ELECT_GEAR", 0)]
checks += [("ELECT_GEAR", 20)]
signals.append(("Elect_Gear_Shifter", "ELECT_GEAR"))
checks.append(("ELECT_GEAR", 20))
else:
signals += [
("CF_Lvr_Gear", "LVR12", 0)
]
checks += [
("LVR12", 100)
]
signals.append(("CF_Lvr_Gear", "LVR12"))
checks.append(("LVR12", 100))
return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 0)
@staticmethod
def get_cam_can_parser(CP):
signals = [
# sig_name, sig_address, default
("CF_Lkas_LdwsActivemode", "LKAS11", 0),
("CF_Lkas_LdwsSysState", "LKAS11", 0),
("CF_Lkas_SysWarning", "LKAS11", 0),
("CF_Lkas_LdwsLHWarning", "LKAS11", 0),
("CF_Lkas_LdwsRHWarning", "LKAS11", 0),
("CF_Lkas_HbaLamp", "LKAS11", 0),
("CF_Lkas_FcwBasReq", "LKAS11", 0),
("CF_Lkas_HbaSysState", "LKAS11", 0),
("CF_Lkas_FcwOpt", "LKAS11", 0),
("CF_Lkas_HbaOpt", "LKAS11", 0),
("CF_Lkas_FcwSysState", "LKAS11", 0),
("CF_Lkas_FcwCollisionWarning", "LKAS11", 0),
("CF_Lkas_FusionState", "LKAS11", 0),
("CF_Lkas_FcwOpt_USM", "LKAS11", 0),
("CF_Lkas_LdwsOpt_USM", "LKAS11", 0),
# sig_name, sig_address
("CF_Lkas_LdwsActivemode", "LKAS11"),
("CF_Lkas_LdwsSysState", "LKAS11"),
("CF_Lkas_SysWarning", "LKAS11"),
("CF_Lkas_LdwsLHWarning", "LKAS11"),
("CF_Lkas_LdwsRHWarning", "LKAS11"),
("CF_Lkas_HbaLamp", "LKAS11"),
("CF_Lkas_FcwBasReq", "LKAS11"),
("CF_Lkas_HbaSysState", "LKAS11"),
("CF_Lkas_FcwOpt", "LKAS11"),
("CF_Lkas_HbaOpt", "LKAS11"),
("CF_Lkas_FcwSysState", "LKAS11"),
("CF_Lkas_FcwCollisionWarning", "LKAS11"),
("CF_Lkas_FusionState", "LKAS11"),
("CF_Lkas_FcwOpt_USM", "LKAS11"),
("CF_Lkas_LdwsOpt_USM", "LKAS11"),
]
checks = [

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

@ -347,7 +347,7 @@ class CarInterface(CarInterfaceBase):
def apply(self, c):
hud_control = c.hudControl
ret = self.CC.update(c.enabled, self.CS, self.frame, c.actuators,
ret = self.CC.update(c, c.enabled, self.CS, self.frame, c.actuators,
c.cruiseControl.cancel, hud_control.visualAlert, hud_control.setSpeed, hud_control.leftLaneVisible,
hud_control.rightLaneVisible, hud_control.leftLaneDepart, hud_control.rightLaneDepart)
self.frame += 1

10
selfdrive/car/hyundai/radar_interface.py
Unescape View File

@ -20,11 +20,11 @@ def get_radar_can_parser(CP):
for addr in range(RADAR_START_ADDR, RADAR_START_ADDR + RADAR_MSG_COUNT):
msg = f"RADAR_TRACK_{addr:x}"
signals += [
("STATE", msg, 0),
("AZIMUTH", msg, 0),
("LONG_DIST", msg, 0),
("REL_ACCEL", msg, 0),
("REL_SPEED", msg, 0),
("STATE", msg),
("AZIMUTH", msg),
("LONG_DIST", msg),
("REL_ACCEL", msg),
("REL_SPEED", msg),
]
checks += [(msg, 50)]
return CANParser(DBC[CP.carFingerprint]['radar'], signals, checks, 1)

8
selfdrive/car/hyundai/values.py
Unescape View File

@ -1,5 +1,3 @@
# flake8: noqa
from cereal import car
from selfdrive.car import dbc_dict
Ecu = car.CarParams.Ecu
@ -672,7 +670,7 @@ FW_VERSIONS = {
b'\xf1\x00IK MDPS R 1.00 1.07 57700-G9220 4I2VL107',
],
(Ecu.transmission, 0x7e1, None): [
b'\xf1\x87VCJLP18407832DN3\x88vXfvUVT\x97eFU\x87d7v\x88eVeveFU\x89\x98\x7f\xff\xb2\xb0\xf1\x81E25\x00\x00\x00'
b'\xf1\x87VCJLP18407832DN3\x88vXfvUVT\x97eFU\x87d7v\x88eVeveFU\x89\x98\x7f\xff\xb2\xb0\xf1\x81E25\x00\x00\x00',
b'\x00\x00\x00\x00\xf1\x00bcsh8p54 E25\x00\x00\x00\x00\x00\x00\x00SIK0T33NB4\xecE\xefL',
],
(Ecu.fwdRadar, 0x7d0, None): [
@ -798,7 +796,7 @@ FW_VERSIONS = {
b'\xf1\x00DEev SCC F-CUP 1.00 1.03 96400-Q4100 ',
b'\xf1\x8799110Q4000\xf1\x00DEev SCC F-CUP 1.00 1.00 99110-Q4000 ',
b'\xf1\x8799110Q4100\xf1\x00DEev SCC F-CUP 1.00 1.00 99110-Q4100 ',
b'\xf1\x8799110Q4500\xf1\000DEev SCC F-CUP 1.00 1.00 99110-Q4500 ',
b'\xf1\x8799110Q4500\xf1\x00DEev SCC F-CUP 1.00 1.00 99110-Q4500 ',
b'\xf1\x8799110Q4600\xf1\x00DEev SCC FNCUP 1.00 1.00 99110-Q4600 ',
b'\xf1\x8799110Q4600\xf1\x00DEev SCC FHCUP 1.00 1.00 99110-Q4600 ',
],
@ -807,7 +805,7 @@ FW_VERSIONS = {
b'\xf1\x00DE MDPS C 1.00 1.05 56310Q4100\x00 4DEEC105',
],
(Ecu.fwdCamera, 0x7C4, None): [
b'\xf1\000DEE MFC AT EUR LHD 1.00 1.00 99211-Q4100 200706',
b'\xf1\x00DEE MFC AT EUR LHD 1.00 1.00 99211-Q4100 200706',
b'\xf1\x00DEE MFC AT EUR LHD 1.00 1.00 99211-Q4000 191211',
b'\xf1\x00DEE MFC AT USA LHD 1.00 1.00 99211-Q4000 191211',
b'\xf1\x00DEE MFC AT USA LHD 1.00 1.03 95740-Q4000 180821',

8
selfdrive/car/interfaces.py
Unescape View File

@ -72,6 +72,7 @@ class CarInterfaceBase(ABC):
def get_std_params(candidate, fingerprint):
ret = car.CarParams.new_message()
ret.carFingerprint = candidate
ret.unsafeMode = 0 # see safety_declarations.h for allowed values
# standard ALC params
ret.steerControlType = car.CarParams.SteerControlType.torque
@ -97,6 +98,7 @@ class CarInterfaceBase(ABC):
ret.longitudinalTuning.kiV = [1.]
ret.longitudinalActuatorDelayLowerBound = 0.15
ret.longitudinalActuatorDelayUpperBound = 0.15
ret.steerLimitTimer = 1.0
return ret
@abstractmethod
@ -107,7 +109,7 @@ class CarInterfaceBase(ABC):
def apply(self, c: car.CarControl) -> Tuple[car.CarControl.Actuators, List[bytes]]:
pass
def create_common_events(self, cs_out, extra_gears=None, gas_resume_speed=-1, pcm_enable=True):
def create_common_events(self, cs_out, extra_gears=None, pcm_enable=True):
events = Events()
if cs_out.doorOpen:
@ -152,9 +154,7 @@ class CarInterfaceBase(ABC):
events.add(EventName.steerUnavailable)
# Disable on rising edge of gas or brake. Also disable on brake when speed > 0.
# Optionally allow to press gas at zero speed to resume.
# e.g. Chrysler does not spam the resume button yet, so resuming with gas is handy. FIXME!
if (cs_out.gasPressed and (not self.CS.out.gasPressed) and cs_out.vEgo > gas_resume_speed) or \
if (cs_out.gasPressed and not self.CS.out.gasPressed) or \
(cs_out.brakePressed and (not self.CS.out.brakePressed or not cs_out.standstill)):
events.add(EventName.pedalPressed)

2
selfdrive/car/mazda/carcontroller.py
Unescape View File

@ -19,7 +19,7 @@ class CarController():
apply_steer = 0
self.steer_rate_limited = False
if c.enabled:
if c.active:
# calculate steer and also set limits due to driver torque
new_steer = int(round(c.actuators.steer * CarControllerParams.STEER_MAX))
apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last,

125
selfdrive/car/mazda/carstate.py
Unescape View File

@ -16,6 +16,7 @@ class CarState(CarStateBase):
self.acc_active_last = False
self.low_speed_alert = False
self.lkas_allowed_speed = False
self.lkas_disabled = False
def update(self, cp, cp_cam):
@ -64,12 +65,13 @@ class CarState(CarStateBase):
# Either due to low speed or hands off
lkas_blocked = cp.vl["STEER_RATE"]["LKAS_BLOCK"] == 1
# LKAS is enabled at 52kph going up and disabled at 45kph going down
# wait for LKAS_BLOCK signal to clear when going up since it lags behind the speed sometimes
if speed_kph > LKAS_LIMITS.ENABLE_SPEED and not lkas_blocked:
self.lkas_allowed_speed = True
elif speed_kph < LKAS_LIMITS.DISABLE_SPEED:
self.lkas_allowed_speed = False
if self.CP.minSteerSpeed > 0:
# LKAS is enabled at 52kph going up and disabled at 45kph going down
# wait for LKAS_BLOCK signal to clear when going up since it lags behind the speed sometimes
if speed_kph > LKAS_LIMITS.ENABLE_SPEED and not lkas_blocked:
self.lkas_allowed_speed = True
elif speed_kph < LKAS_LIMITS.DISABLE_SPEED:
self.lkas_allowed_speed = False
# TODO: the signal used for available seems to be the adaptive cruise signal, instead of the main on
# it should be used for carState.cruiseState.nonAdaptive instead
@ -86,34 +88,35 @@ class CarState(CarStateBase):
# Check if LKAS is disabled due to lack of driver torque when all other states indicate
# it should be enabled (steer lockout). Don't warn until we actually get lkas active
# and lose it again, i.e, after initial lkas activation
ret.steerWarning = self.lkas_allowed_speed and lkas_blocked
self.acc_active_last = ret.cruiseState.enabled
self.crz_btns_counter = cp.vl["CRZ_BTNS"]["CTR"]
# camera signals
self.lkas_disabled = cp_cam.vl["CAM_LANEINFO"]["LANE_LINES"] == 0
self.cam_lkas = cp_cam.vl["CAM_LKAS"]
self.cam_laneinfo = cp_cam.vl["CAM_LANEINFO"]
self.crz_btns_counter = cp.vl["CRZ_BTNS"]["CTR"]
ret.steerError = cp_cam.vl["CAM_LKAS"]["ERR_BIT_1"] == 1
return ret
@staticmethod
def get_can_parser(CP):
# this function generates lists for signal, messages and initial values
signals = [
# sig_name, sig_address, default
("LEFT_BLINK", "BLINK_INFO", 0),
("RIGHT_BLINK", "BLINK_INFO", 0),
("HIGH_BEAMS", "BLINK_INFO", 0),
("STEER_ANGLE", "STEER", 0),
("STEER_ANGLE_RATE", "STEER_RATE", 0),
("STEER_TORQUE_SENSOR", "STEER_TORQUE", 0),
("STEER_TORQUE_MOTOR", "STEER_TORQUE", 0),
("FL", "WHEEL_SPEEDS", 0),
("FR", "WHEEL_SPEEDS", 0),
("RL", "WHEEL_SPEEDS", 0),
("RR", "WHEEL_SPEEDS", 0),
# sig_name, sig_address
("LEFT_BLINK", "BLINK_INFO"),
("RIGHT_BLINK", "BLINK_INFO"),
("HIGH_BEAMS", "BLINK_INFO"),
("STEER_ANGLE", "STEER"),
("STEER_ANGLE_RATE", "STEER_RATE"),
("STEER_TORQUE_SENSOR", "STEER_TORQUE"),
("STEER_TORQUE_MOTOR", "STEER_TORQUE"),
("FL", "WHEEL_SPEEDS"),
("FR", "WHEEL_SPEEDS"),
("RL", "WHEEL_SPEEDS"),
("RR", "WHEEL_SPEEDS"),
]
checks = [
@ -127,26 +130,26 @@ class CarState(CarStateBase):
if CP.carFingerprint in GEN1:
signals += [
("LKAS_BLOCK", "STEER_RATE", 0),
("LKAS_TRACK_STATE", "STEER_RATE", 0),
("HANDS_OFF_5_SECONDS", "STEER_RATE", 0),
("CRZ_ACTIVE", "CRZ_CTRL", 0),
("CRZ_AVAILABLE", "CRZ_CTRL", 0),
("CRZ_SPEED", "CRZ_EVENTS", 0),
("STANDSTILL", "PEDALS", 0),
("BRAKE_ON", "PEDALS", 0),
("BRAKE_PRESSURE", "BRAKE", 0),
("GEAR", "GEAR", 0),
("DRIVER_SEATBELT", "SEATBELT", 0),
("FL", "DOORS", 0),
("FR", "DOORS", 0),
("BL", "DOORS", 0),
("BR", "DOORS", 0),
("PEDAL_GAS", "ENGINE_DATA", 0),
("SPEED", "ENGINE_DATA", 0),
("CTR", "CRZ_BTNS", 0),
("LEFT_BS1", "BSM", 0),
("RIGHT_BS1", "BSM", 0),
("LKAS_BLOCK", "STEER_RATE"),
("LKAS_TRACK_STATE", "STEER_RATE"),
("HANDS_OFF_5_SECONDS", "STEER_RATE"),
("CRZ_ACTIVE", "CRZ_CTRL"),
("CRZ_AVAILABLE", "CRZ_CTRL"),
("CRZ_SPEED", "CRZ_EVENTS"),
("STANDSTILL", "PEDALS"),
("BRAKE_ON", "PEDALS"),
("BRAKE_PRESSURE", "BRAKE"),
("GEAR", "GEAR"),
("DRIVER_SEATBELT", "SEATBELT"),
("FL", "DOORS"),
("FR", "DOORS"),
("BL", "DOORS"),
("BR", "DOORS"),
("PEDAL_GAS", "ENGINE_DATA"),
("SPEED", "ENGINE_DATA"),
("CTR", "CRZ_BTNS"),
("LEFT_BS1", "BSM"),
("RIGHT_BS1", "BSM"),
]
checks += [
@ -171,26 +174,26 @@ class CarState(CarStateBase):
if CP.carFingerprint in GEN1:
signals += [
# sig_name, sig_address, default
("LKAS_REQUEST", "CAM_LKAS", 0),
("CTR", "CAM_LKAS", 0),
("ERR_BIT_1", "CAM_LKAS", 0),
("LINE_NOT_VISIBLE", "CAM_LKAS", 0),
("BIT_1", "CAM_LKAS", 1),
("ERR_BIT_2", "CAM_LKAS", 0),
("STEERING_ANGLE", "CAM_LKAS", 0),
("ANGLE_ENABLED", "CAM_LKAS", 0),
("CHKSUM", "CAM_LKAS", 0),
("LINE_VISIBLE", "CAM_LANEINFO", 0),
("LINE_NOT_VISIBLE", "CAM_LANEINFO", 1),
("LANE_LINES", "CAM_LANEINFO", 0),
("BIT1", "CAM_LANEINFO", 0),
("BIT2", "CAM_LANEINFO", 0),
("BIT3", "CAM_LANEINFO", 0),
("NO_ERR_BIT", "CAM_LANEINFO", 1),
("S1", "CAM_LANEINFO", 0),
("S1_HBEAM", "CAM_LANEINFO", 0),
# sig_name, sig_address
("LKAS_REQUEST", "CAM_LKAS"),
("CTR", "CAM_LKAS"),
("ERR_BIT_1", "CAM_LKAS"),
("LINE_NOT_VISIBLE", "CAM_LKAS"),
("BIT_1", "CAM_LKAS"),
("ERR_BIT_2", "CAM_LKAS"),
("STEERING_ANGLE", "CAM_LKAS"),
("ANGLE_ENABLED", "CAM_LKAS"),
("CHKSUM", "CAM_LKAS"),
("LINE_VISIBLE", "CAM_LANEINFO"),
("LINE_NOT_VISIBLE", "CAM_LANEINFO"),
("LANE_LINES", "CAM_LANEINFO"),
("BIT1", "CAM_LANEINFO"),
("BIT2", "CAM_LANEINFO"),
("BIT3", "CAM_LANEINFO"),
("NO_ERR_BIT", "CAM_LANEINFO"),
("S1", "CAM_LANEINFO"),
("S1_HBEAM", "CAM_LANEINFO"),
]
checks += [

12
selfdrive/car/mazda/interface.py
Unescape View File

@ -22,14 +22,14 @@ class CarInterface(CarInterfaceBase):
ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.mazda)]
ret.radarOffCan = True
ret.dashcamOnly = candidate not in (CAR.CX9_2021,)
ret.dashcamOnly = candidate not in (CAR.CX5_2022, CAR.CX9_2021)
ret.steerActuatorDelay = 0.1
ret.steerRateCost = 1.0
ret.steerLimitTimer = 0.8
tire_stiffness_factor = 0.70 # not optimized yet
if candidate == CAR.CX5:
if candidate in (CAR.CX5, CAR.CX5_2022):
ret.mass = 3655 * CV.LB_TO_KG + STD_CARGO_KG
ret.wheelbase = 2.7
ret.steerRatio = 15.5
@ -58,8 +58,8 @@ class CarInterface(CarInterfaceBase):
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.19], [0.019]]
ret.lateralTuning.pid.kf = 0.00006
# No steer below disable speed
ret.minSteerSpeed = LKAS_LIMITS.DISABLE_SPEED * CV.KPH_TO_MS
if candidate not in (CAR.CX5_2022, ):
ret.minSteerSpeed = LKAS_LIMITS.DISABLE_SPEED * CV.KPH_TO_MS
ret.centerToFront = ret.wheelbase * 0.41
@ -86,7 +86,9 @@ class CarInterface(CarInterfaceBase):
# events
events = self.create_common_events(ret)
if self.CS.low_speed_alert:
if self.CS.lkas_disabled:
events.add(EventName.lkasDisabled)
elif self.CS.low_speed_alert:
events.add(EventName.belowSteerSpeed)
ret.events = events.to_msg()

31
selfdrive/car/mazda/values.py
Unescape View File

@ -1,5 +1,3 @@
# flake8: noqa
from selfdrive.car import dbc_dict
from cereal import car
Ecu = car.CarParams.Ecu
@ -21,7 +19,8 @@ class CAR:
CX9 = "MAZDA CX-9"
MAZDA3 = "MAZDA 3"
MAZDA6 = "MAZDA 6"
CX9_2021 = "MAZDA CX-9 2021" # No Steer Lockout
CX9_2021 = "MAZDA CX-9 2021"
CX5_2022 = "MAZDA CX-5 2022"
class LKAS_LIMITS:
STEER_THRESHOLD = 15
@ -37,6 +36,26 @@ class Buttons:
FW_VERSIONS = {
CAR.CX5_2022 : {
(Ecu.eps, 0x730, None): [
b'KSD5-3210X-C-00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.engine, 0x7e0, None): [
b'PX2G-188K2-H\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.fwdRadar, 0x764, None): [
b'K131-67XK2-F\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.esp, 0x760, None): [
b'KSD5-437K2-A\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.fwdCamera, 0x706, None): [
b'GSH7-67XK2-S\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.transmission, 0x7e1, None): [
b'PYB2-21PS1-H\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
},
CAR.CX5: {
(Ecu.eps, 0x730, None): [
b'KJ01-3210X-G-00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
@ -244,10 +263,8 @@ DBC = {
CAR.MAZDA3: dbc_dict('mazda_2017', None),
CAR.MAZDA6: dbc_dict('mazda_2017', None),
CAR.CX9_2021: dbc_dict('mazda_2017', None),
CAR.CX5_2022: dbc_dict('mazda_2017', None),
}
# Gen 1 hardware: same CAN messages and same camera
GEN1 = {CAR.CX5, CAR.CX9, CAR.CX9_2021, CAR.MAZDA3, CAR.MAZDA6}
# Cars with a steering lockout
STEER_LOCKOUT_CAR = {CAR.CX5, CAR.CX9, CAR.MAZDA3, CAR.MAZDA6}
GEN1 = {CAR.CX5, CAR.CX9, CAR.CX9_2021, CAR.MAZDA3, CAR.MAZDA6, CAR.CX5_2022}

8
selfdrive/car/nissan/carcontroller.py
Unescape View File

@ -18,22 +18,20 @@ class CarController():
self.packer = CANPacker(dbc_name)
def update(self, enabled, CS, frame, actuators, cruise_cancel, hud_alert,
def update(self, c, enabled, CS, frame, actuators, cruise_cancel, hud_alert,
left_line, right_line, left_lane_depart, right_lane_depart):
""" Controls thread """
# Send CAN commands.
can_sends = []
### STEER ###
acc_active = bool(CS.out.cruiseState.enabled)
acc_active = CS.out.cruiseState.enabled
lkas_hud_msg = CS.lkas_hud_msg
lkas_hud_info_msg = CS.lkas_hud_info_msg
apply_angle = actuators.steeringAngleDeg
steer_hud_alert = 1 if hud_alert in (VisualAlert.steerRequired, VisualAlert.ldw) else 0
if enabled:
if c.active:
# # windup slower
if self.last_angle * apply_angle > 0. and abs(apply_angle) > abs(self.last_angle):
angle_rate_lim = interp(CS.out.vEgo, CarControllerParams.ANGLE_DELTA_BP, CarControllerParams.ANGLE_DELTA_V)

284
selfdrive/car/nissan/carstate.py
Unescape View File

@ -119,27 +119,26 @@ class CarState(CarStateBase):
@staticmethod
def get_can_parser(CP):
# this function generates lists for signal, messages and initial values
signals = [
# sig_name, sig_address, default
("WHEEL_SPEED_FL", "WHEEL_SPEEDS_FRONT", 0),
("WHEEL_SPEED_FR", "WHEEL_SPEEDS_FRONT", 0),
("WHEEL_SPEED_RL", "WHEEL_SPEEDS_REAR", 0),
("WHEEL_SPEED_RR", "WHEEL_SPEEDS_REAR", 0),
# sig_name, sig_address
("WHEEL_SPEED_FL", "WHEEL_SPEEDS_FRONT"),
("WHEEL_SPEED_FR", "WHEEL_SPEEDS_FRONT"),
("WHEEL_SPEED_RL", "WHEEL_SPEEDS_REAR"),
("WHEEL_SPEED_RR", "WHEEL_SPEEDS_REAR"),
("STEER_ANGLE", "STEER_ANGLE_SENSOR", 0),
("STEER_ANGLE", "STEER_ANGLE_SENSOR"),
("DOOR_OPEN_FR", "DOORS_LIGHTS", 1),
("DOOR_OPEN_FL", "DOORS_LIGHTS", 1),
("DOOR_OPEN_RR", "DOORS_LIGHTS", 1),
("DOOR_OPEN_RL", "DOORS_LIGHTS", 1),
("DOOR_OPEN_FR", "DOORS_LIGHTS"),
("DOOR_OPEN_FL", "DOORS_LIGHTS"),
("DOOR_OPEN_RR", "DOORS_LIGHTS"),
("DOOR_OPEN_RL", "DOORS_LIGHTS"),
("RIGHT_BLINKER", "LIGHTS", 0),
("LEFT_BLINKER", "LIGHTS", 0),
("RIGHT_BLINKER", "LIGHTS"),
("LEFT_BLINKER", "LIGHTS"),
("ESP_DISABLED", "ESP", 0),
("ESP_DISABLED", "ESP"),
("GEAR_SHIFTER", "GEARBOX", 0),
("GEAR_SHIFTER", "GEARBOX"),
]
checks = [
@ -155,26 +154,26 @@ class CarState(CarStateBase):
if CP.carFingerprint in (CAR.ROGUE, CAR.XTRAIL, CAR.ALTIMA):
signals += [
("USER_BRAKE_PRESSED", "DOORS_LIGHTS", 1),
("GAS_PEDAL", "GAS_PEDAL", 0),
("SEATBELT_DRIVER_LATCHED", "HUD", 0),
("SPEED_MPH", "HUD", 0),
("PROPILOT_BUTTON", "CRUISE_THROTTLE", 0),
("CANCEL_BUTTON", "CRUISE_THROTTLE", 0),
("GAS_PEDAL_INVERTED", "CRUISE_THROTTLE", 0),
("SET_BUTTON", "CRUISE_THROTTLE", 0),
("RES_BUTTON", "CRUISE_THROTTLE", 0),
("FOLLOW_DISTANCE_BUTTON", "CRUISE_THROTTLE", 0),
("NO_BUTTON_PRESSED", "CRUISE_THROTTLE", 0),
("GAS_PEDAL", "CRUISE_THROTTLE", 0),
("USER_BRAKE_PRESSED", "CRUISE_THROTTLE", 0),
("NEW_SIGNAL_2", "CRUISE_THROTTLE", 0),
("GAS_PRESSED_INVERTED", "CRUISE_THROTTLE", 0),
("unsure1", "CRUISE_THROTTLE", 0),
("unsure2", "CRUISE_THROTTLE", 0),
("unsure3", "CRUISE_THROTTLE", 0),
("USER_BRAKE_PRESSED", "DOORS_LIGHTS"),
("GAS_PEDAL", "GAS_PEDAL"),
("SEATBELT_DRIVER_LATCHED", "HUD"),
("SPEED_MPH", "HUD"),
("PROPILOT_BUTTON", "CRUISE_THROTTLE"),
("CANCEL_BUTTON", "CRUISE_THROTTLE"),
("GAS_PEDAL_INVERTED", "CRUISE_THROTTLE"),
("SET_BUTTON", "CRUISE_THROTTLE"),
("RES_BUTTON", "CRUISE_THROTTLE"),
("FOLLOW_DISTANCE_BUTTON", "CRUISE_THROTTLE"),
("NO_BUTTON_PRESSED", "CRUISE_THROTTLE"),
("GAS_PEDAL", "CRUISE_THROTTLE"),
("USER_BRAKE_PRESSED", "CRUISE_THROTTLE"),
("NEW_SIGNAL_2", "CRUISE_THROTTLE"),
("GAS_PRESSED_INVERTED", "CRUISE_THROTTLE"),
("unsure1", "CRUISE_THROTTLE"),
("unsure2", "CRUISE_THROTTLE"),
("unsure3", "CRUISE_THROTTLE"),
]
checks += [
@ -185,17 +184,17 @@ class CarState(CarStateBase):
elif CP.carFingerprint in (CAR.LEAF, CAR.LEAF_IC):
signals += [
("USER_BRAKE_PRESSED", "CRUISE_THROTTLE", 1),
("GAS_PEDAL", "CRUISE_THROTTLE", 0),
("CRUISE_AVAILABLE", "CRUISE_THROTTLE", 0),
("SPEED_MPH", "HUD_SETTINGS", 0),
("SEATBELT_DRIVER_LATCHED", "SEATBELT", 0),
("USER_BRAKE_PRESSED", "CRUISE_THROTTLE"),
("GAS_PEDAL", "CRUISE_THROTTLE"),
("CRUISE_AVAILABLE", "CRUISE_THROTTLE"),
("SPEED_MPH", "HUD_SETTINGS"),
("SEATBELT_DRIVER_LATCHED", "SEATBELT"),
# Copy other values, we use this to cancel
("CANCEL_SEATBELT", "CANCEL_MSG", 0),
("NEW_SIGNAL_1", "CANCEL_MSG", 0),
("NEW_SIGNAL_2", "CANCEL_MSG", 0),
("NEW_SIGNAL_3", "CANCEL_MSG", 0),
("CANCEL_SEATBELT", "CANCEL_MSG"),
("NEW_SIGNAL_1", "CANCEL_MSG"),
("NEW_SIGNAL_2", "CANCEL_MSG"),
("NEW_SIGNAL_3", "CANCEL_MSG"),
]
checks += [
("BRAKE_PEDAL", 100),
@ -207,9 +206,9 @@ class CarState(CarStateBase):
if CP.carFingerprint == CAR.ALTIMA:
signals += [
("LKAS_ENABLED", "LKAS_SETTINGS", 0),
("CRUISE_ENABLED", "CRUISE_STATE", 0),
("SET_SPEED", "PROPILOT_HUD", 0),
("LKAS_ENABLED", "LKAS_SETTINGS"),
("CRUISE_ENABLED", "CRUISE_STATE"),
("SET_SPEED", "PROPILOT_HUD"),
]
checks += [
("CRUISE_STATE", 10),
@ -218,12 +217,8 @@ class CarState(CarStateBase):
]
return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 1)
signals += [
("STEER_TORQUE_DRIVER", "STEER_TORQUE_SENSOR", 0),
]
checks += [
("STEER_TORQUE_SENSOR", 100),
]
signals.append(("STEER_TORQUE_DRIVER", "STEER_TORQUE_SENSOR"))
checks.append(("STEER_TORQUE_SENSOR", 100))
return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 0)
@ -233,14 +228,14 @@ class CarState(CarStateBase):
if CP.carFingerprint == CAR.ALTIMA:
signals = [
("DESIRED_ANGLE", "LKAS", 0),
("SET_0x80_2", "LKAS", 0),
("MAX_TORQUE", "LKAS", 0),
("SET_0x80", "LKAS", 0),
("COUNTER", "LKAS", 0),
("LKA_ACTIVE", "LKAS", 0),
("CRUISE_ON", "PRO_PILOT", 0),
("DESIRED_ANGLE", "LKAS"),
("SET_0x80_2", "LKAS"),
("MAX_TORQUE", "LKAS"),
("SET_0x80", "LKAS"),
("COUNTER", "LKAS"),
("LKA_ACTIVE", "LKAS"),
("CRUISE_ON", "PRO_PILOT"),
]
checks = [
("LKAS", 100),
@ -248,85 +243,85 @@ class CarState(CarStateBase):
]
else:
signals = [
# sig_name, sig_address, default
("LKAS_ENABLED", "LKAS_SETTINGS", 0),
# sig_name, sig_address
("LKAS_ENABLED", "LKAS_SETTINGS"),
("CRUISE_ENABLED", "CRUISE_STATE", 0),
("CRUISE_ENABLED", "CRUISE_STATE"),
("DESIRED_ANGLE", "LKAS", 0),
("SET_0x80_2", "LKAS", 0),
("MAX_TORQUE", "LKAS", 0),
("SET_0x80", "LKAS", 0),
("COUNTER", "LKAS", 0),
("LKA_ACTIVE", "LKAS", 0),
("DESIRED_ANGLE", "LKAS"),
("SET_0x80_2", "LKAS"),
("MAX_TORQUE", "LKAS"),
("SET_0x80", "LKAS"),
("COUNTER", "LKAS"),
("LKA_ACTIVE", "LKAS"),
# Below are the HUD messages. We copy the stock message and modify
("LARGE_WARNING_FLASHING", "PROPILOT_HUD", 0),
("SIDE_RADAR_ERROR_FLASHING1", "PROPILOT_HUD", 0),
("SIDE_RADAR_ERROR_FLASHING2", "PROPILOT_HUD", 0),
("LEAD_CAR", "PROPILOT_HUD", 0),
("LEAD_CAR_ERROR", "PROPILOT_HUD", 0),
("FRONT_RADAR_ERROR", "PROPILOT_HUD", 0),
("FRONT_RADAR_ERROR_FLASHING", "PROPILOT_HUD", 0),
("SIDE_RADAR_ERROR_FLASHING3", "PROPILOT_HUD", 0),
("LKAS_ERROR_FLASHING", "PROPILOT_HUD", 0),
("SAFETY_SHIELD_ACTIVE", "PROPILOT_HUD", 0),
("RIGHT_LANE_GREEN_FLASH", "PROPILOT_HUD", 0),
("LEFT_LANE_GREEN_FLASH", "PROPILOT_HUD", 0),
("FOLLOW_DISTANCE", "PROPILOT_HUD", 0),
("AUDIBLE_TONE", "PROPILOT_HUD", 0),
("SPEED_SET_ICON", "PROPILOT_HUD", 0),
("SMALL_STEERING_WHEEL_ICON", "PROPILOT_HUD", 0),
("unknown59", "PROPILOT_HUD", 0),
("unknown55", "PROPILOT_HUD", 0),
("unknown26", "PROPILOT_HUD", 0),
("unknown28", "PROPILOT_HUD", 0),
("unknown31", "PROPILOT_HUD", 0),
("SET_SPEED", "PROPILOT_HUD", 0),
("unknown43", "PROPILOT_HUD", 0),
("unknown08", "PROPILOT_HUD", 0),
("unknown05", "PROPILOT_HUD", 0),
("unknown02", "PROPILOT_HUD", 0),
("NA_HIGH_ACCEL_TEMP", "PROPILOT_HUD_INFO_MSG", 0),
("SIDE_RADAR_NA_HIGH_CABIN_TEMP", "PROPILOT_HUD_INFO_MSG", 0),
("SIDE_RADAR_MALFUNCTION", "PROPILOT_HUD_INFO_MSG", 0),
("LKAS_MALFUNCTION", "PROPILOT_HUD_INFO_MSG", 0),
("FRONT_RADAR_MALFUNCTION", "PROPILOT_HUD_INFO_MSG", 0),
("SIDE_RADAR_NA_CLEAN_REAR_CAMERA", "PROPILOT_HUD_INFO_MSG", 0),
("NA_POOR_ROAD_CONDITIONS", "PROPILOT_HUD_INFO_MSG", 0),
("CURRENTLY_UNAVAILABLE", "PROPILOT_HUD_INFO_MSG", 0),
("SAFETY_SHIELD_OFF", "PROPILOT_HUD_INFO_MSG", 0),
("FRONT_COLLISION_NA_FRONT_RADAR_OBSTRUCTION", "PROPILOT_HUD_INFO_MSG", 0),
("PEDAL_MISSAPPLICATION_SYSTEM_ACTIVATED", "PROPILOT_HUD_INFO_MSG", 0),
("SIDE_IMPACT_NA_RADAR_OBSTRUCTION", "PROPILOT_HUD_INFO_MSG", 0),
("WARNING_DO_NOT_ENTER", "PROPILOT_HUD_INFO_MSG", 0),
("SIDE_IMPACT_SYSTEM_OFF", "PROPILOT_HUD_INFO_MSG", 0),
("SIDE_IMPACT_MALFUNCTION", "PROPILOT_HUD_INFO_MSG", 0),
("FRONT_COLLISION_MALFUNCTION", "PROPILOT_HUD_INFO_MSG", 0),
("SIDE_RADAR_MALFUNCTION2", "PROPILOT_HUD_INFO_MSG", 0),
("LKAS_MALFUNCTION2", "PROPILOT_HUD_INFO_MSG", 0),
("FRONT_RADAR_MALFUNCTION2", "PROPILOT_HUD_INFO_MSG", 0),
("PROPILOT_NA_MSGS", "PROPILOT_HUD_INFO_MSG", 0),
("BOTTOM_MSG", "PROPILOT_HUD_INFO_MSG", 0),
("HANDS_ON_WHEEL_WARNING", "PROPILOT_HUD_INFO_MSG", 0),
("WARNING_STEP_ON_BRAKE_NOW", "PROPILOT_HUD_INFO_MSG", 0),
("PROPILOT_NA_FRONT_CAMERA_OBSTRUCTED", "PROPILOT_HUD_INFO_MSG", 0),
("PROPILOT_NA_HIGH_CABIN_TEMP", "PROPILOT_HUD_INFO_MSG", 0),
("WARNING_PROPILOT_MALFUNCTION", "PROPILOT_HUD_INFO_MSG", 0),
("ACC_UNAVAILABLE_HIGH_CABIN_TEMP", "PROPILOT_HUD_INFO_MSG", 0),
("ACC_NA_FRONT_CAMERA_IMPARED", "PROPILOT_HUD_INFO_MSG", 0),
("unknown07", "PROPILOT_HUD_INFO_MSG", 0),
("unknown10", "PROPILOT_HUD_INFO_MSG", 0),
("unknown15", "PROPILOT_HUD_INFO_MSG", 0),
("unknown23", "PROPILOT_HUD_INFO_MSG", 0),
("unknown19", "PROPILOT_HUD_INFO_MSG", 0),
("unknown31", "PROPILOT_HUD_INFO_MSG", 0),
("unknown32", "PROPILOT_HUD_INFO_MSG", 0),
("unknown46", "PROPILOT_HUD_INFO_MSG", 0),
("unknown61", "PROPILOT_HUD_INFO_MSG", 0),
("unknown55", "PROPILOT_HUD_INFO_MSG", 0),
("unknown50", "PROPILOT_HUD_INFO_MSG", 0),
("LARGE_WARNING_FLASHING", "PROPILOT_HUD"),
("SIDE_RADAR_ERROR_FLASHING1", "PROPILOT_HUD"),
("SIDE_RADAR_ERROR_FLASHING2", "PROPILOT_HUD"),
("LEAD_CAR", "PROPILOT_HUD"),
("LEAD_CAR_ERROR", "PROPILOT_HUD"),
("FRONT_RADAR_ERROR", "PROPILOT_HUD"),
("FRONT_RADAR_ERROR_FLASHING", "PROPILOT_HUD"),
("SIDE_RADAR_ERROR_FLASHING3", "PROPILOT_HUD"),
("LKAS_ERROR_FLASHING", "PROPILOT_HUD"),
("SAFETY_SHIELD_ACTIVE", "PROPILOT_HUD"),
("RIGHT_LANE_GREEN_FLASH", "PROPILOT_HUD"),
("LEFT_LANE_GREEN_FLASH", "PROPILOT_HUD"),
("FOLLOW_DISTANCE", "PROPILOT_HUD"),
("AUDIBLE_TONE", "PROPILOT_HUD"),
("SPEED_SET_ICON", "PROPILOT_HUD"),
("SMALL_STEERING_WHEEL_ICON", "PROPILOT_HUD"),
("unknown59", "PROPILOT_HUD"),
("unknown55", "PROPILOT_HUD"),
("unknown26", "PROPILOT_HUD"),
("unknown28", "PROPILOT_HUD"),
("unknown31", "PROPILOT_HUD"),
("SET_SPEED", "PROPILOT_HUD"),
("unknown43", "PROPILOT_HUD"),
("unknown08", "PROPILOT_HUD"),
("unknown05", "PROPILOT_HUD"),
("unknown02", "PROPILOT_HUD"),
("NA_HIGH_ACCEL_TEMP", "PROPILOT_HUD_INFO_MSG"),
("SIDE_RADAR_NA_HIGH_CABIN_TEMP", "PROPILOT_HUD_INFO_MSG"),
("SIDE_RADAR_MALFUNCTION", "PROPILOT_HUD_INFO_MSG"),
("LKAS_MALFUNCTION", "PROPILOT_HUD_INFO_MSG"),
("FRONT_RADAR_MALFUNCTION", "PROPILOT_HUD_INFO_MSG"),
("SIDE_RADAR_NA_CLEAN_REAR_CAMERA", "PROPILOT_HUD_INFO_MSG"),
("NA_POOR_ROAD_CONDITIONS", "PROPILOT_HUD_INFO_MSG"),
("CURRENTLY_UNAVAILABLE", "PROPILOT_HUD_INFO_MSG"),
("SAFETY_SHIELD_OFF", "PROPILOT_HUD_INFO_MSG"),
("FRONT_COLLISION_NA_FRONT_RADAR_OBSTRUCTION", "PROPILOT_HUD_INFO_MSG"),
("PEDAL_MISSAPPLICATION_SYSTEM_ACTIVATED", "PROPILOT_HUD_INFO_MSG"),
("SIDE_IMPACT_NA_RADAR_OBSTRUCTION", "PROPILOT_HUD_INFO_MSG"),
("WARNING_DO_NOT_ENTER", "PROPILOT_HUD_INFO_MSG"),
("SIDE_IMPACT_SYSTEM_OFF", "PROPILOT_HUD_INFO_MSG"),
("SIDE_IMPACT_MALFUNCTION", "PROPILOT_HUD_INFO_MSG"),
("FRONT_COLLISION_MALFUNCTION", "PROPILOT_HUD_INFO_MSG"),
("SIDE_RADAR_MALFUNCTION2", "PROPILOT_HUD_INFO_MSG"),
("LKAS_MALFUNCTION2", "PROPILOT_HUD_INFO_MSG"),
("FRONT_RADAR_MALFUNCTION2", "PROPILOT_HUD_INFO_MSG"),
("PROPILOT_NA_MSGS", "PROPILOT_HUD_INFO_MSG"),
("BOTTOM_MSG", "PROPILOT_HUD_INFO_MSG"),
("HANDS_ON_WHEEL_WARNING", "PROPILOT_HUD_INFO_MSG"),
("WARNING_STEP_ON_BRAKE_NOW", "PROPILOT_HUD_INFO_MSG"),
("PROPILOT_NA_FRONT_CAMERA_OBSTRUCTED", "PROPILOT_HUD_INFO_MSG"),
("PROPILOT_NA_HIGH_CABIN_TEMP", "PROPILOT_HUD_INFO_MSG"),
("WARNING_PROPILOT_MALFUNCTION", "PROPILOT_HUD_INFO_MSG"),
("ACC_UNAVAILABLE_HIGH_CABIN_TEMP", "PROPILOT_HUD_INFO_MSG"),
("ACC_NA_FRONT_CAMERA_IMPARED", "PROPILOT_HUD_INFO_MSG"),
("unknown07", "PROPILOT_HUD_INFO_MSG"),
("unknown10", "PROPILOT_HUD_INFO_MSG"),
("unknown15", "PROPILOT_HUD_INFO_MSG"),
("unknown23", "PROPILOT_HUD_INFO_MSG"),
("unknown19", "PROPILOT_HUD_INFO_MSG"),
("unknown31", "PROPILOT_HUD_INFO_MSG"),
("unknown32", "PROPILOT_HUD_INFO_MSG"),
("unknown46", "PROPILOT_HUD_INFO_MSG"),
("unknown61", "PROPILOT_HUD_INFO_MSG"),
("unknown55", "PROPILOT_HUD_INFO_MSG"),
("unknown50", "PROPILOT_HUD_INFO_MSG"),
]
checks = [
@ -345,20 +340,11 @@ class CarState(CarStateBase):
checks = []
if CP.carFingerprint in (CAR.ROGUE, CAR.XTRAIL):
signals += [
("CRUISE_ON", "PRO_PILOT", 0),
]
checks += [
("PRO_PILOT", 100),
]
signals.append(("CRUISE_ON", "PRO_PILOT"))
checks.append(("PRO_PILOT", 100))
elif CP.carFingerprint == CAR.ALTIMA:
signals += [
("STEER_TORQUE_DRIVER", "STEER_TORQUE_SENSOR", 0),
]
checks += [
("STEER_TORQUE_SENSOR", 100),
]
signals.append(("STEER_TORQUE_DRIVER", "STEER_TORQUE_SENSOR"))
checks.append(("STEER_TORQUE_SENSOR", 100))
return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 0)
return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 1)

4
selfdrive/car/nissan/interface.py
Unescape View File

@ -16,7 +16,7 @@ class CarInterface(CarInterfaceBase):
ret.carName = "nissan"
ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.nissan)]
ret.steerLimitAlert = False
ret.steerLimitTimer = 1.0
ret.steerRateCost = 0.5
ret.steerActuatorDelay = 0.1
@ -79,7 +79,7 @@ class CarInterface(CarInterfaceBase):
def apply(self, c):
hud_control = c.hudControl
ret = self.CC.update(c.enabled, self.CS, self.frame, c.actuators,
ret = self.CC.update(c, c.enabled, self.CS, self.frame, c.actuators,
c.cruiseControl.cancel, hud_control.visualAlert,
hud_control.leftLaneVisible, hud_control.rightLaneVisible,
hud_control.leftLaneDepart, hud_control.rightLaneDepart)

2
selfdrive/car/nissan/values.py
Unescape View File

@ -1,5 +1,3 @@
# flake8: noqa
from selfdrive.car import dbc_dict
from cereal import car
Ecu = car.CarParams.Ecu

4
selfdrive/car/subaru/carcontroller.py
Unescape View File

@ -15,7 +15,7 @@ class CarController():
self.p = CarControllerParams(CP)
self.packer = CANPacker(DBC[CP.carFingerprint]['pt'])
def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, visual_alert, left_line, right_line, left_lane_depart, right_lane_depart):
def update(self, c, enabled, CS, frame, actuators, pcm_cancel_cmd, visual_alert, left_line, right_line, left_lane_depart, right_lane_depart):
can_sends = []
@ -30,7 +30,7 @@ class CarController():
apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, self.p)
self.steer_rate_limited = new_steer != apply_steer
if not enabled:
if not c.active:
apply_steer = 0
if CS.CP.carFingerprint in PREGLOBAL_CARS:

179
selfdrive/car/subaru/carstate.py
Unescape View File

@ -77,28 +77,27 @@ class CarState(CarStateBase):
@staticmethod
def get_can_parser(CP):
# this function generates lists for signal, messages and initial values
signals = [
# sig_name, sig_address, default
("Steer_Torque_Sensor", "Steering_Torque", 0),
("Steering_Angle", "Steering_Torque", 0),
("Steer_Error_1", "Steering_Torque", 0),
("Cruise_On", "CruiseControl", 0),
("Cruise_Activated", "CruiseControl", 0),
("Brake_Pedal", "Brake_Pedal", 0),
("Throttle_Pedal", "Throttle", 0),
("LEFT_BLINKER", "Dashlights", 0),
("RIGHT_BLINKER", "Dashlights", 0),
("SEATBELT_FL", "Dashlights", 0),
("FL", "Wheel_Speeds", 0),
("FR", "Wheel_Speeds", 0),
("RL", "Wheel_Speeds", 0),
("RR", "Wheel_Speeds", 0),
("DOOR_OPEN_FR", "BodyInfo", 1),
("DOOR_OPEN_FL", "BodyInfo", 1),
("DOOR_OPEN_RR", "BodyInfo", 1),
("DOOR_OPEN_RL", "BodyInfo", 1),
("Gear", "Transmission", 0),
# sig_name, sig_address
("Steer_Torque_Sensor", "Steering_Torque"),
("Steering_Angle", "Steering_Torque"),
("Steer_Error_1", "Steering_Torque"),
("Cruise_On", "CruiseControl"),
("Cruise_Activated", "CruiseControl"),
("Brake_Pedal", "Brake_Pedal"),
("Throttle_Pedal", "Throttle"),
("LEFT_BLINKER", "Dashlights"),
("RIGHT_BLINKER", "Dashlights"),
("SEATBELT_FL", "Dashlights"),
("FL", "Wheel_Speeds"),
("FR", "Wheel_Speeds"),
("RL", "Wheel_Speeds"),
("RR", "Wheel_Speeds"),
("DOOR_OPEN_FR", "BodyInfo"),
("DOOR_OPEN_FL", "BodyInfo"),
("DOOR_OPEN_RR", "BodyInfo"),
("DOOR_OPEN_RL", "BodyInfo"),
("Gear", "Transmission"),
]
checks = [
@ -114,20 +113,18 @@ class CarState(CarStateBase):
if CP.enableBsm:
signals += [
("L_ADJACENT", "BSD_RCTA", 0),
("R_ADJACENT", "BSD_RCTA", 0),
("L_APPROACHING", "BSD_RCTA", 0),
("R_APPROACHING", "BSD_RCTA", 0),
]
checks += [
("BSD_RCTA", 17),
("L_ADJACENT", "BSD_RCTA"),
("R_ADJACENT", "BSD_RCTA"),
("L_APPROACHING", "BSD_RCTA"),
("R_APPROACHING", "BSD_RCTA"),
]
checks.append(("BSD_RCTA", 17))
if CP.carFingerprint not in PREGLOBAL_CARS:
signals += [
("Steer_Warning", "Steering_Torque", 0),
("Brake", "Brake_Status", 0),
("UNITS", "Dashlights", 0),
("Steer_Warning", "Steering_Torque"),
("Brake", "Brake_Status"),
("UNITS", "Dashlights"),
]
checks += [
@ -137,13 +134,9 @@ class CarState(CarStateBase):
("CruiseControl", 20),
]
else:
signals += [
("UNITS", "Dash_State2", 0),
]
signals.append(("UNITS", "Dash_State2"))
checks += [
("Dash_State2", 1),
]
checks.append(("Dash_State2", 1))
if CP.carFingerprint == CAR.FORESTER_PREGLOBAL:
checks += [
@ -164,26 +157,26 @@ class CarState(CarStateBase):
def get_cam_can_parser(CP):
if CP.carFingerprint in PREGLOBAL_CARS:
signals = [
("Cruise_Set_Speed", "ES_DashStatus", 0),
("Not_Ready_Startup", "ES_DashStatus", 0),
("Cruise_Throttle", "ES_Distance", 0),
("Signal1", "ES_Distance", 0),
("Car_Follow", "ES_Distance", 0),
("Signal2", "ES_Distance", 0),
("Brake_On", "ES_Distance", 0),
("Distance_Swap", "ES_Distance", 0),
("Standstill", "ES_Distance", 0),
("Signal3", "ES_Distance", 0),
("Close_Distance", "ES_Distance", 0),
("Signal4", "ES_Distance", 0),
("Standstill_2", "ES_Distance", 0),
("Cruise_Fault", "ES_Distance", 0),
("Signal5", "ES_Distance", 0),
("Counter", "ES_Distance", 0),
("Signal6", "ES_Distance", 0),
("Cruise_Button", "ES_Distance", 0),
("Signal7", "ES_Distance", 0),
("Cruise_Set_Speed", "ES_DashStatus"),
("Not_Ready_Startup", "ES_DashStatus"),
("Cruise_Throttle", "ES_Distance"),
("Signal1", "ES_Distance"),
("Car_Follow", "ES_Distance"),
("Signal2", "ES_Distance"),
("Brake_On", "ES_Distance"),
("Distance_Swap", "ES_Distance"),
("Standstill", "ES_Distance"),
("Signal3", "ES_Distance"),
("Close_Distance", "ES_Distance"),
("Signal4", "ES_Distance"),
("Standstill_2", "ES_Distance"),
("Cruise_Fault", "ES_Distance"),
("Signal5", "ES_Distance"),
("Counter", "ES_Distance"),
("Signal6", "ES_Distance"),
("Cruise_Button", "ES_Distance"),
("Signal7", "ES_Distance"),
]
checks = [
@ -192,42 +185,42 @@ class CarState(CarStateBase):
]
else:
signals = [
("Cruise_Set_Speed", "ES_DashStatus", 0),
("Conventional_Cruise", "ES_DashStatus", 0),
("Counter", "ES_Distance", 0),
("Signal1", "ES_Distance", 0),
("Cruise_Fault", "ES_Distance", 0),
("Cruise_Throttle", "ES_Distance", 0),
("Signal2", "ES_Distance", 0),
("Car_Follow", "ES_Distance", 0),
("Signal3", "ES_Distance", 0),
("Cruise_Brake_Active", "ES_Distance", 0),
("Distance_Swap", "ES_Distance", 0),
("Cruise_EPB", "ES_Distance", 0),
("Signal4", "ES_Distance", 0),
("Close_Distance", "ES_Distance", 0),
("Signal5", "ES_Distance", 0),
("Cruise_Cancel", "ES_Distance", 0),
("Cruise_Set", "ES_Distance", 0),
("Cruise_Resume", "ES_Distance", 0),
("Signal6", "ES_Distance", 0),
("Counter", "ES_LKAS_State", 0),
("LKAS_Alert_Msg", "ES_LKAS_State", 0),
("Signal1", "ES_LKAS_State", 0),
("LKAS_ACTIVE", "ES_LKAS_State", 0),
("LKAS_Dash_State", "ES_LKAS_State", 0),
("Signal2", "ES_LKAS_State", 0),
("Backward_Speed_Limit_Menu", "ES_LKAS_State", 0),
("LKAS_Left_Line_Enable", "ES_LKAS_State", 0),
("LKAS_Left_Line_Light_Blink", "ES_LKAS_State", 0),
("LKAS_Right_Line_Enable", "ES_LKAS_State", 0),
("LKAS_Right_Line_Light_Blink", "ES_LKAS_State", 0),
("LKAS_Left_Line_Visible", "ES_LKAS_State", 0),
("LKAS_Right_Line_Visible", "ES_LKAS_State", 0),
("LKAS_Alert", "ES_LKAS_State", 0),
("Signal3", "ES_LKAS_State", 0),
("Cruise_Set_Speed", "ES_DashStatus"),
("Conventional_Cruise", "ES_DashStatus"),
("Counter", "ES_Distance"),
("Signal1", "ES_Distance"),
("Cruise_Fault", "ES_Distance"),
("Cruise_Throttle", "ES_Distance"),
("Signal2", "ES_Distance"),
("Car_Follow", "ES_Distance"),
("Signal3", "ES_Distance"),
("Cruise_Brake_Active", "ES_Distance"),
("Distance_Swap", "ES_Distance"),
("Cruise_EPB", "ES_Distance"),
("Signal4", "ES_Distance"),
("Close_Distance", "ES_Distance"),
("Signal5", "ES_Distance"),
("Cruise_Cancel", "ES_Distance"),
("Cruise_Set", "ES_Distance"),
("Cruise_Resume", "ES_Distance"),
("Signal6", "ES_Distance"),
("Counter", "ES_LKAS_State"),
("LKAS_Alert_Msg", "ES_LKAS_State"),
("Signal1", "ES_LKAS_State"),
("LKAS_ACTIVE", "ES_LKAS_State"),
("LKAS_Dash_State", "ES_LKAS_State"),
("Signal2", "ES_LKAS_State"),
("Backward_Speed_Limit_Menu", "ES_LKAS_State"),
("LKAS_Left_Line_Enable", "ES_LKAS_State"),
("LKAS_Left_Line_Light_Blink", "ES_LKAS_State"),
("LKAS_Right_Line_Enable", "ES_LKAS_State"),
("LKAS_Right_Line_Light_Blink", "ES_LKAS_State"),
("LKAS_Left_Line_Visible", "ES_LKAS_State"),
("LKAS_Right_Line_Visible", "ES_LKAS_State"),
("LKAS_Alert", "ES_LKAS_State"),
("Signal3", "ES_LKAS_State"),
]
checks = [

2
selfdrive/car/subaru/interface.py
Unescape View File

@ -123,7 +123,7 @@ class CarInterface(CarInterfaceBase):
def apply(self, c):
hud_control = c.hudControl
ret = self.CC.update(c.enabled, self.CS, self.frame, c.actuators,
ret = self.CC.update(c, c.enabled, self.CS, self.frame, c.actuators,
c.cruiseControl.cancel, hud_control.visualAlert,
hud_control.leftLaneVisible, hud_control.rightLaneVisible, hud_control.leftLaneDepart, hud_control.rightLaneDepart)
self.frame += 1

10
selfdrive/car/subaru/values.py
Unescape View File

@ -1,5 +1,3 @@
# flake8: noqa
from selfdrive.car import dbc_dict
from cereal import car
Ecu = car.CarParams.Ecu
@ -112,8 +110,7 @@ FW_VERSIONS = {
b'\xaa!`u\a',
b'\xaa!dq\a',
b'\xaa!dt\a',
b'\xf1\x00\xa2\x10\t'
b'\xc5!dr\a',
b'\xf1\x00\xa2\x10\t',
b'\xc5!ar\a',
b'\xbe!as\a',
b'\xc5!ds\a',
@ -131,7 +128,6 @@ FW_VERSIONS = {
b'\xe3\xf5G\x00\x00',
b'\xe3\xf5\a\x00\x00',
b'\xe3\xf5C\x00\x00',
b'\xf1\x00\xa4\x10@'
b'\xe5\xf5B\x00\x00',
b'\xe5\xf5$\000\000',
b'\xe4\xf5\a\000\000',
@ -197,7 +193,7 @@ FW_VERSIONS = {
b'\032\xf6B0\000',
b'\032\xf6F`\000',
b'\032\xf6b`\000',
b'\032\xf6B`\000'
b'\032\xf6B`\000',
b'\xf1\x00\xa4\x10@',
],
},
@ -298,7 +294,7 @@ FW_VERSIONS = {
b'\xab"@s\a',
b'\xab+@@\a',
b'\xb4"@r\a',
b'\xa0+@@\x07'
b'\xa0+@@\x07',
b'\xa0\"@\x80\a',
],
(Ecu.transmission, 0x7e1, None): [

6
selfdrive/car/tesla/carcontroller.py
Unescape View File

@ -12,12 +12,12 @@ class CarController():
self.pt_packer = CANPacker(DBC[CP.carFingerprint]['pt'])
self.tesla_can = TeslaCAN(self.packer, self.pt_packer)
def update(self, enabled, CS, frame, actuators, cruise_cancel):
def update(self, c, enabled, CS, frame, actuators, cruise_cancel):
can_sends = []
# Temp disable steering on a hands_on_fault, and allow for user override
hands_on_fault = (CS.steer_warning == "EAC_ERROR_HANDS_ON" and CS.hands_on_level >= 3)
lkas_enabled = enabled and (not hands_on_fault)
lkas_enabled = c.active and (not hands_on_fault)
if lkas_enabled:
apply_angle = actuators.steeringAngleDeg
@ -65,4 +65,4 @@ class CarController():
new_actuators = actuators.copy()
new_actuators.steeringAngleDeg = apply_angle
return actuators, can_sends
return new_actuators, can_sends

116
selfdrive/car/tesla/carstate.py
Unescape View File

@ -96,64 +96,64 @@ class CarState(CarStateBase):
@staticmethod
def get_can_parser(CP):
signals = [
# sig_name, sig_address, default
("ESP_vehicleSpeed", "ESP_B", 0),
("DI_pedalPos", "DI_torque1", 0),
("DI_brakePedal", "DI_torque2", 0),
("StW_AnglHP", "STW_ANGLHP_STAT", 0),
("StW_AnglHP_Spd", "STW_ANGLHP_STAT", 0),
("EPAS_handsOnLevel", "EPAS_sysStatus", 0),
("EPAS_torsionBarTorque", "EPAS_sysStatus", 0),
("EPAS_internalSAS", "EPAS_sysStatus", 0),
("EPAS_eacStatus", "EPAS_sysStatus", 1),
("EPAS_eacErrorCode", "EPAS_sysStatus", 0),
("DI_cruiseState", "DI_state", 0),
("DI_digitalSpeed", "DI_state", 0),
("DI_speedUnits", "DI_state", 0),
("DI_gear", "DI_torque2", 0),
("DOOR_STATE_FL", "GTW_carState", 1),
("DOOR_STATE_FR", "GTW_carState", 1),
("DOOR_STATE_RL", "GTW_carState", 1),
("DOOR_STATE_RR", "GTW_carState", 1),
("DOOR_STATE_FrontTrunk", "GTW_carState", 1),
("BOOT_STATE", "GTW_carState", 1),
("BC_indicatorLStatus", "GTW_carState", 1),
("BC_indicatorRStatus", "GTW_carState", 1),
("SDM_bcklDrivStatus", "SDM1", 0),
("driverBrakeStatus", "BrakeMessage", 0),
# sig_name, sig_address
("ESP_vehicleSpeed", "ESP_B"),
("DI_pedalPos", "DI_torque1"),
("DI_brakePedal", "DI_torque2"),
("StW_AnglHP", "STW_ANGLHP_STAT"),
("StW_AnglHP_Spd", "STW_ANGLHP_STAT"),
("EPAS_handsOnLevel", "EPAS_sysStatus"),
("EPAS_torsionBarTorque", "EPAS_sysStatus"),
("EPAS_internalSAS", "EPAS_sysStatus"),
("EPAS_eacStatus", "EPAS_sysStatus"),
("EPAS_eacErrorCode", "EPAS_sysStatus"),
("DI_cruiseState", "DI_state"),
("DI_digitalSpeed", "DI_state"),
("DI_speedUnits", "DI_state"),
("DI_gear", "DI_torque2"),
("DOOR_STATE_FL", "GTW_carState"),
("DOOR_STATE_FR", "GTW_carState"),
("DOOR_STATE_RL", "GTW_carState"),
("DOOR_STATE_RR", "GTW_carState"),
("DOOR_STATE_FrontTrunk", "GTW_carState"),
("BOOT_STATE", "GTW_carState"),
("BC_indicatorLStatus", "GTW_carState"),
("BC_indicatorRStatus", "GTW_carState"),
("SDM_bcklDrivStatus", "SDM1"),
("driverBrakeStatus", "BrakeMessage"),
# We copy this whole message when spamming cancel
("SpdCtrlLvr_Stat", "STW_ACTN_RQ", 0),
("VSL_Enbl_Rq", "STW_ACTN_RQ", 0),
("SpdCtrlLvrStat_Inv", "STW_ACTN_RQ", 0),
("DTR_Dist_Rq", "STW_ACTN_RQ", 0),
("TurnIndLvr_Stat", "STW_ACTN_RQ", 0),
("HiBmLvr_Stat", "STW_ACTN_RQ", 0),
("WprWashSw_Psd", "STW_ACTN_RQ", 0),
("WprWash_R_Sw_Posn_V2", "STW_ACTN_RQ", 0),
("StW_Lvr_Stat", "STW_ACTN_RQ", 0),
("StW_Cond_Flt", "STW_ACTN_RQ", 0),
("StW_Cond_Psd", "STW_ACTN_RQ", 0),
("HrnSw_Psd", "STW_ACTN_RQ", 0),
("StW_Sw00_Psd", "STW_ACTN_RQ", 0),
("StW_Sw01_Psd", "STW_ACTN_RQ", 0),
("StW_Sw02_Psd", "STW_ACTN_RQ", 0),
("StW_Sw03_Psd", "STW_ACTN_RQ", 0),
("StW_Sw04_Psd", "STW_ACTN_RQ", 0),
("StW_Sw05_Psd", "STW_ACTN_RQ", 0),
("StW_Sw06_Psd", "STW_ACTN_RQ", 0),
("StW_Sw07_Psd", "STW_ACTN_RQ", 0),
("StW_Sw08_Psd", "STW_ACTN_RQ", 0),
("StW_Sw09_Psd", "STW_ACTN_RQ", 0),
("StW_Sw10_Psd", "STW_ACTN_RQ", 0),
("StW_Sw11_Psd", "STW_ACTN_RQ", 0),
("StW_Sw12_Psd", "STW_ACTN_RQ", 0),
("StW_Sw13_Psd", "STW_ACTN_RQ", 0),
("StW_Sw14_Psd", "STW_ACTN_RQ", 0),
("StW_Sw15_Psd", "STW_ACTN_RQ", 0),
("WprSw6Posn", "STW_ACTN_RQ", 0),
("MC_STW_ACTN_RQ", "STW_ACTN_RQ", 0),
("CRC_STW_ACTN_RQ", "STW_ACTN_RQ", 0),
("SpdCtrlLvr_Stat", "STW_ACTN_RQ"),
("VSL_Enbl_Rq", "STW_ACTN_RQ"),
("SpdCtrlLvrStat_Inv", "STW_ACTN_RQ"),
("DTR_Dist_Rq", "STW_ACTN_RQ"),
("TurnIndLvr_Stat", "STW_ACTN_RQ"),
("HiBmLvr_Stat", "STW_ACTN_RQ"),
("WprWashSw_Psd", "STW_ACTN_RQ"),
("WprWash_R_Sw_Posn_V2", "STW_ACTN_RQ"),
("StW_Lvr_Stat", "STW_ACTN_RQ"),
("StW_Cond_Flt", "STW_ACTN_RQ"),
("StW_Cond_Psd", "STW_ACTN_RQ"),
("HrnSw_Psd", "STW_ACTN_RQ"),
("StW_Sw00_Psd", "STW_ACTN_RQ"),
("StW_Sw01_Psd", "STW_ACTN_RQ"),
("StW_Sw02_Psd", "STW_ACTN_RQ"),
("StW_Sw03_Psd", "STW_ACTN_RQ"),
("StW_Sw04_Psd", "STW_ACTN_RQ"),
("StW_Sw05_Psd", "STW_ACTN_RQ"),
("StW_Sw06_Psd", "STW_ACTN_RQ"),
("StW_Sw07_Psd", "STW_ACTN_RQ"),
("StW_Sw08_Psd", "STW_ACTN_RQ"),
("StW_Sw09_Psd", "STW_ACTN_RQ"),
("StW_Sw10_Psd", "STW_ACTN_RQ"),
("StW_Sw11_Psd", "STW_ACTN_RQ"),
("StW_Sw12_Psd", "STW_ACTN_RQ"),
("StW_Sw13_Psd", "STW_ACTN_RQ"),
("StW_Sw14_Psd", "STW_ACTN_RQ"),
("StW_Sw15_Psd", "STW_ACTN_RQ"),
("WprSw6Posn", "STW_ACTN_RQ"),
("MC_STW_ACTN_RQ", "STW_ACTN_RQ"),
("CRC_STW_ACTN_RQ", "STW_ACTN_RQ"),
]
checks = [
@ -175,8 +175,8 @@ class CarState(CarStateBase):
@staticmethod
def get_cam_can_parser(CP):
signals = [
# sig_name, sig_address, default
("DAS_accState", "DAS_control", 0),
# sig_name, sig_address
("DAS_accState", "DAS_control"),
]
checks = [
# sig_address, frequency

3
selfdrive/car/tesla/interface.py
Unescape View File

@ -40,6 +40,7 @@ class CarInterface(CarInterfaceBase):
ret.openpilotLongitudinalControl = False
ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.tesla, 0)]
ret.steerLimitTimer = 1.0
ret.steerActuatorDelay = 0.1
ret.steerRateCost = 0.5
@ -70,6 +71,6 @@ class CarInterface(CarInterfaceBase):
return self.CS.out
def apply(self, c):
ret = self.CC.update(c.enabled, self.CS, self.frame, c.actuators, c.cruiseControl.cancel)
ret = self.CC.update(c, c.enabled, self.CS, self.frame, c.actuators, c.cruiseControl.cancel)
self.frame += 1
return ret

26
selfdrive/car/tesla/radar_interface.py
Unescape View File

@ -11,9 +11,9 @@ NUM_POINTS = len(RADAR_MSGS_A)
def get_radar_can_parser(CP):
# Status messages
signals = [
('RADC_HWFail', 'TeslaRadarSguInfo', 0),
('RADC_SGUFail', 'TeslaRadarSguInfo', 0),
('RADC_SensorDirty', 'TeslaRadarSguInfo', 0),
('RADC_HWFail', 'TeslaRadarSguInfo'),
('RADC_SGUFail', 'TeslaRadarSguInfo'),
('RADC_SensorDirty', 'TeslaRadarSguInfo'),
]
checks = [
@ -29,16 +29,16 @@ def get_radar_can_parser(CP):
# There is a bunch more info in the messages,
# but these are the only things actually used in openpilot
signals.extend([
('LongDist', msg_id_a, 255),
('LongSpeed', msg_id_a, 0),
('LatDist', msg_id_a, 0),
('LongAccel', msg_id_a, 0),
('Meas', msg_id_a, 0),
('Tracked', msg_id_a, 0),
('Index', msg_id_a, 0),
('LatSpeed', msg_id_b, 0),
('Index2', msg_id_b, 0),
('LongDist', msg_id_a),
('LongSpeed', msg_id_a),
('LatDist', msg_id_a),
('LongAccel', msg_id_a),
('Meas', msg_id_a),
('Tracked', msg_id_a),
('Index', msg_id_a),
('LatSpeed', msg_id_b),
('Index2', msg_id_b),
])
checks.extend([

2
selfdrive/car/tesla/values.py
Unescape View File

@ -1,5 +1,3 @@
# flake8: noqa
from collections import namedtuple
from selfdrive.car import dbc_dict
from cereal import car

6
selfdrive/car/toyota/carcontroller.py
Unescape View File

@ -26,7 +26,7 @@ class CarController():
left_line, right_line, lead, left_lane_depart, right_lane_depart):
# gas and brake
if CS.CP.enableGasInterceptor and enabled:
if CS.CP.enableGasInterceptor and active:
MAX_INTERCEPTOR_GAS = 0.5
# RAV4 has very sensitive gas pedal
if CS.CP.carFingerprint in (CAR.RAV4, CAR.RAV4H, CAR.HIGHLANDER, CAR.HIGHLANDERH):
@ -49,7 +49,7 @@ class CarController():
self.steer_rate_limited = new_steer != apply_steer
# Cut steering while we're in a known fault state (2s)
if not enabled or CS.steer_state in (9, 25):
if not active or CS.steer_state in (9, 25):
apply_steer = 0
apply_steer_req = 0
else:
@ -106,7 +106,7 @@ class CarController():
can_sends.append(create_gas_interceptor_command(self.packer, interceptor_gas_cmd, frame // 2))
self.gas = interceptor_gas_cmd
# ui mesg is at 100Hz but we send asap if:
# ui mesg is at 1Hz but we send asap if:
# - there is something to display
# - there is something to stop displaying
fcw_alert = hud_alert == VisualAlert.fcw

118
selfdrive/car/toyota/carstate.py
Unescape View File

@ -6,7 +6,7 @@ from opendbc.can.can_define import CANDefine
from selfdrive.car.interfaces import CarStateBase
from opendbc.can.parser import CANParser
from selfdrive.config import Conversions as CV
from selfdrive.car.toyota.values import CAR, DBC, STEER_THRESHOLD, NO_STOP_TIMER_CAR, TSS2_CAR
from selfdrive.car.toyota.values import ToyotaFlags, CAR, DBC, STEER_THRESHOLD, NO_STOP_TIMER_CAR, TSS2_CAR, EPS_SCALE
class CarState(CarStateBase):
@ -14,6 +14,7 @@ class CarState(CarStateBase):
super().__init__(CP)
can_define = CANDefine(DBC[CP.carFingerprint]["pt"])
self.shifter_values = can_define.dv["GEAR_PACKET"]["GEAR"]
self.eps_torque_scale = EPS_SCALE[CP.carFingerprint] / 100.
# On cars with cp.vl["STEER_TORQUE_SENSOR"]["STEER_ANGLE"]
# the signal is zeroed to where the steering angle is at start.
@ -28,9 +29,9 @@ class CarState(CarStateBase):
def update(self, cp, cp_cam):
ret = car.CarState.new_message()
ret.doorOpen = any([cp.vl["SEATS_DOORS"]["DOOR_OPEN_FL"], cp.vl["SEATS_DOORS"]["DOOR_OPEN_FR"],
cp.vl["SEATS_DOORS"]["DOOR_OPEN_RL"], cp.vl["SEATS_DOORS"]["DOOR_OPEN_RR"]])
ret.seatbeltUnlatched = cp.vl["SEATS_DOORS"]["SEATBELT_DRIVER_UNLATCHED"] != 0
ret.doorOpen = any([cp.vl["BODY_CONTROL_STATE"]["DOOR_OPEN_FL"], cp.vl["BODY_CONTROL_STATE"]["DOOR_OPEN_FR"],
cp.vl["BODY_CONTROL_STATE"]["DOOR_OPEN_RL"], cp.vl["BODY_CONTROL_STATE"]["DOOR_OPEN_RR"]])
ret.seatbeltUnlatched = cp.vl["BODY_CONTROL_STATE"]["SEATBELT_DRIVER_UNLATCHED"] != 0
ret.brakePressed = cp.vl["BRAKE_MODULE"]["BRAKE_PRESSED"] != 0
ret.brakeHoldActive = cp.vl["ESP_CONTROL"]["BRAKE_HOLD_ACTIVE"] == 1
@ -38,7 +39,9 @@ class CarState(CarStateBase):
ret.gas = (cp.vl["GAS_SENSOR"]["INTERCEPTOR_GAS"] + cp.vl["GAS_SENSOR"]["INTERCEPTOR_GAS2"]) / 2.
ret.gasPressed = ret.gas > 15
else:
ret.gas = cp.vl["GAS_PEDAL"]["GAS_PEDAL"]
# TODO: find a new, common signal
msg = "GAS_PEDAL_HYBRID" if (self.CP.flags & ToyotaFlags.HYBRID) else "GAS_PEDAL"
ret.gas = cp.vl[msg]["GAS_PEDAL"]
ret.gasPressed = cp.vl["PCM_CRUISE"]["GAS_RELEASED"] == 0
ret.wheelSpeeds = self.get_wheel_speeds(
@ -72,11 +75,11 @@ class CarState(CarStateBase):
can_gear = int(cp.vl["GEAR_PACKET"]["GEAR"])
ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(can_gear, None))
ret.leftBlinker = cp.vl["STEERING_LEVERS"]["TURN_SIGNALS"] == 1
ret.rightBlinker = cp.vl["STEERING_LEVERS"]["TURN_SIGNALS"] == 2
ret.leftBlinker = cp.vl["BLINKERS_STATE"]["TURN_SIGNALS"] == 1
ret.rightBlinker = cp.vl["BLINKERS_STATE"]["TURN_SIGNALS"] == 2
ret.steeringTorque = cp.vl["STEER_TORQUE_SENSOR"]["STEER_TORQUE_DRIVER"]
ret.steeringTorqueEps = cp.vl["STEER_TORQUE_SENSOR"]["STEER_TORQUE_EPS"]
ret.steeringTorqueEps = cp.vl["STEER_TORQUE_SENSOR"]["STEER_TORQUE_EPS"] * self.eps_torque_scale
# we could use the override bit from dbc, but it's triggered at too high torque values
ret.steeringPressed = abs(ret.steeringTorque) > STEER_THRESHOLD
ret.steerWarning = cp.vl["EPS_STATUS"]["LKA_STATE"] not in (1, 5)
@ -124,87 +127,88 @@ class CarState(CarStateBase):
@staticmethod
def get_can_parser(CP):
signals = [
# sig_name, sig_address, default
("STEER_ANGLE", "STEER_ANGLE_SENSOR", 0),
("GEAR", "GEAR_PACKET", 0),
("BRAKE_PRESSED", "BRAKE_MODULE", 0),
("GAS_PEDAL", "GAS_PEDAL", 0),
("WHEEL_SPEED_FL", "WHEEL_SPEEDS", 0),
("WHEEL_SPEED_FR", "WHEEL_SPEEDS", 0),
("WHEEL_SPEED_RL", "WHEEL_SPEEDS", 0),
("WHEEL_SPEED_RR", "WHEEL_SPEEDS", 0),
("DOOR_OPEN_FL", "SEATS_DOORS", 1),
("DOOR_OPEN_FR", "SEATS_DOORS", 1),
("DOOR_OPEN_RL", "SEATS_DOORS", 1),
("DOOR_OPEN_RR", "SEATS_DOORS", 1),
("SEATBELT_DRIVER_UNLATCHED", "SEATS_DOORS", 1),
("TC_DISABLED", "ESP_CONTROL", 1),
("BRAKE_HOLD_ACTIVE", "ESP_CONTROL", 1),
("STEER_FRACTION", "STEER_ANGLE_SENSOR", 0),
("STEER_RATE", "STEER_ANGLE_SENSOR", 0),
("CRUISE_ACTIVE", "PCM_CRUISE", 0),
("CRUISE_STATE", "PCM_CRUISE", 0),
("GAS_RELEASED", "PCM_CRUISE", 1),
("STEER_TORQUE_DRIVER", "STEER_TORQUE_SENSOR", 0),
("STEER_TORQUE_EPS", "STEER_TORQUE_SENSOR", 0),
("STEER_ANGLE", "STEER_TORQUE_SENSOR", 0),
("TURN_SIGNALS", "STEERING_LEVERS", 3), # 3 is no blinkers
("LKA_STATE", "EPS_STATUS", 0),
("AUTO_HIGH_BEAM", "LIGHT_STALK", 0),
# sig_name, sig_address
("STEER_ANGLE", "STEER_ANGLE_SENSOR"),
("GEAR", "GEAR_PACKET"),
("BRAKE_PRESSED", "BRAKE_MODULE"),
("WHEEL_SPEED_FL", "WHEEL_SPEEDS"),
("WHEEL_SPEED_FR", "WHEEL_SPEEDS"),
("WHEEL_SPEED_RL", "WHEEL_SPEEDS"),
("WHEEL_SPEED_RR", "WHEEL_SPEEDS"),
("DOOR_OPEN_FL", "BODY_CONTROL_STATE"),
("DOOR_OPEN_FR", "BODY_CONTROL_STATE"),
("DOOR_OPEN_RL", "BODY_CONTROL_STATE"),
("DOOR_OPEN_RR", "BODY_CONTROL_STATE"),
("SEATBELT_DRIVER_UNLATCHED", "BODY_CONTROL_STATE"),
("TC_DISABLED", "ESP_CONTROL"),
("BRAKE_HOLD_ACTIVE", "ESP_CONTROL"),
("STEER_FRACTION", "STEER_ANGLE_SENSOR"),
("STEER_RATE", "STEER_ANGLE_SENSOR"),
("CRUISE_ACTIVE", "PCM_CRUISE"),
("CRUISE_STATE", "PCM_CRUISE"),
("GAS_RELEASED", "PCM_CRUISE"),
("STEER_TORQUE_DRIVER", "STEER_TORQUE_SENSOR"),
("STEER_TORQUE_EPS", "STEER_TORQUE_SENSOR"),
("STEER_ANGLE", "STEER_TORQUE_SENSOR"),
("TURN_SIGNALS", "BLINKERS_STATE"),
("LKA_STATE", "EPS_STATUS"),
("AUTO_HIGH_BEAM", "LIGHT_STALK"),
]
checks = [
("GEAR_PACKET", 1),
("LIGHT_STALK", 1),
("STEERING_LEVERS", 0.15),
("SEATS_DOORS", 3),
("BLINKERS_STATE", 0.15),
("BODY_CONTROL_STATE", 3),
("ESP_CONTROL", 3),
("EPS_STATUS", 25),
("BRAKE_MODULE", 40),
("GAS_PEDAL", 33),
("WHEEL_SPEEDS", 80),
("STEER_ANGLE_SENSOR", 80),
("PCM_CRUISE", 33),
("STEER_TORQUE_SENSOR", 50),
]
if CP.flags & ToyotaFlags.HYBRID:
signals.append(("GAS_PEDAL", "GAS_PEDAL_HYBRID"))
checks.append(("GAS_PEDAL_HYBRID", 33))
else:
signals.append(("GAS_PEDAL", "GAS_PEDAL"))
checks.append(("GAS_PEDAL", 33))
if CP.carFingerprint in (CAR.LEXUS_IS, CAR.LEXUS_RC):
signals.append(("MAIN_ON", "DSU_CRUISE", 0))
signals.append(("SET_SPEED", "DSU_CRUISE", 0))
signals.append(("MAIN_ON", "DSU_CRUISE"))
signals.append(("SET_SPEED", "DSU_CRUISE"))
checks.append(("DSU_CRUISE", 5))
else:
signals.append(("MAIN_ON", "PCM_CRUISE_2", 0))
signals.append(("SET_SPEED", "PCM_CRUISE_2", 0))
signals.append(("LOW_SPEED_LOCKOUT", "PCM_CRUISE_2", 0))
signals.append(("MAIN_ON", "PCM_CRUISE_2"))
signals.append(("SET_SPEED", "PCM_CRUISE_2"))
signals.append(("LOW_SPEED_LOCKOUT", "PCM_CRUISE_2"))
checks.append(("PCM_CRUISE_2", 33))
# add gas interceptor reading if we are using it
if CP.enableGasInterceptor:
signals.append(("INTERCEPTOR_GAS", "GAS_SENSOR", 0))
signals.append(("INTERCEPTOR_GAS2", "GAS_SENSOR", 0))
signals.append(("INTERCEPTOR_GAS", "GAS_SENSOR"))
signals.append(("INTERCEPTOR_GAS2", "GAS_SENSOR"))
checks.append(("GAS_SENSOR", 50))
if CP.enableBsm:
signals += [
("L_ADJACENT", "BSM", 0),
("L_APPROACHING", "BSM", 0),
("R_ADJACENT", "BSM", 0),
("R_APPROACHING", "BSM", 0),
]
checks += [
("BSM", 1)
("L_ADJACENT", "BSM"),
("L_APPROACHING", "BSM"),
("R_ADJACENT", "BSM"),
("R_APPROACHING", "BSM"),
]
checks.append(("BSM", 1))
return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 0)
@staticmethod
def get_cam_can_parser(CP):
signals = [
("FORCE", "PRE_COLLISION", 0),
("PRECOLLISION_ACTIVE", "PRE_COLLISION", 0),
("FORCE", "PRE_COLLISION"),
("PRECOLLISION_ACTIVE", "PRE_COLLISION"),
]
# use steering message to check if panda is connected to frc
@ -214,7 +218,7 @@ class CarState(CarStateBase):
]
if CP.carFingerprint in TSS2_CAR:
signals.append(("ACC_TYPE", "ACC_CONTROL", 0))
signals.append(("ACC_TYPE", "ACC_CONTROL"))
checks.append(("ACC_CONTROL", 33))
return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 2)

54
selfdrive/car/toyota/interface.py
Unescape View File

@ -2,7 +2,7 @@
from cereal import car
from selfdrive.config import Conversions as CV
from selfdrive.car.toyota.tunes import LatTunes, LongTunes, set_long_tune, set_lat_tune
from selfdrive.car.toyota.values import Ecu, CAR, TSS2_CAR, NO_DSU_CAR, MIN_ACC_SPEED, CarControllerParams
from selfdrive.car.toyota.values import Ecu, CAR, ToyotaFlags, TSS2_CAR, NO_DSU_CAR, MIN_ACC_SPEED, EPS_SCALE, CarControllerParams
from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint, get_safety_config
from selfdrive.car.interfaces import CarInterfaceBase
@ -20,17 +20,15 @@ class CarInterface(CarInterfaceBase):
ret.carName = "toyota"
ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.toyota)]
ret.safetyConfigs[0].safetyParam = EPS_SCALE[candidate]
ret.steerActuatorDelay = 0.12 # Default delay, Prius has larger delay
ret.steerLimitTimer = 0.4
ret.stoppingControl = False # Toyota starts braking more when it thinks you want to stop
# Most cars use this default safety param
ret.safetyConfigs[0].safetyParam = 73
stop_and_go = False
if candidate == CAR.PRIUS:
ret.safetyConfigs[0].safetyParam = 66 # see conversion factor for STEER_TORQUE_EPS in dbc file
stop_and_go = True
ret.wheelbase = 2.70
ret.steerRatio = 15.74 # unknown end-to-end spec
@ -40,6 +38,14 @@ class CarInterface(CarInterfaceBase):
set_lat_tune(ret.lateralTuning, LatTunes.INDI_PRIUS)
ret.steerActuatorDelay = 0.3
elif candidate == CAR.PRIUS_V:
stop_and_go = True
ret.wheelbase = 2.78
ret.steerRatio = 17.4
tire_stiffness_factor = 0.5533
ret.mass = 4387. * CV.LB_TO_KG + STD_CARGO_KG
set_lat_tune(ret.lateralTuning, LatTunes.LQR_RAV4)
elif candidate in (CAR.RAV4, CAR.RAV4H):
stop_and_go = True if (candidate in CAR.RAV4H) else False
ret.wheelbase = 2.65
@ -49,8 +55,6 @@ class CarInterface(CarInterfaceBase):
set_lat_tune(ret.lateralTuning, LatTunes.LQR_RAV4)
elif candidate == CAR.COROLLA:
ret.safetyConfigs[0].safetyParam = 88
stop_and_go = False
ret.wheelbase = 2.70
ret.steerRatio = 18.27
tire_stiffness_factor = 0.444 # not optimized yet
@ -98,8 +102,7 @@ class CarInterface(CarInterfaceBase):
ret.mass = 4607. * CV.LB_TO_KG + STD_CARGO_KG # mean between normal and hybrid limited
set_lat_tune(ret.lateralTuning, LatTunes.PID_G)
elif candidate in (CAR.AVALON, CAR.AVALON_2019, CAR.AVALONH_2019):
stop_and_go = False
elif candidate in (CAR.AVALON, CAR.AVALON_2019, CAR.AVALONH_2019, CAR.AVALON_TSS2):
ret.wheelbase = 2.82
ret.steerRatio = 14.8 # Found at https://pressroom.toyota.com/releases/2016+avalon+product+specs.download
tire_stiffness_factor = 0.7983
@ -129,20 +132,12 @@ class CarInterface(CarInterfaceBase):
ret.mass = 3060. * CV.LB_TO_KG + STD_CARGO_KG
set_lat_tune(ret.lateralTuning, LatTunes.PID_D)
elif candidate in (CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2):
elif candidate in (CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2, CAR.LEXUS_ESH):
stop_and_go = True
ret.wheelbase = 2.8702
ret.steerRatio = 16.0 # not optimized
tire_stiffness_factor = 0.444 # not optimized yet
ret.mass = 3704. * CV.LB_TO_KG + STD_CARGO_KG
set_lat_tune(ret.lateralTuning, LatTunes.PID_D)
elif candidate == CAR.LEXUS_ESH:
stop_and_go = True
ret.wheelbase = 2.8190
ret.steerRatio = 16.06
tire_stiffness_factor = 0.444 # not optimized yet
ret.mass = 3682. * CV.LB_TO_KG + STD_CARGO_KG
ret.mass = 3677. * CV.LB_TO_KG + STD_CARGO_KG # mean between min and max
set_lat_tune(ret.lateralTuning, LatTunes.PID_D)
elif candidate == CAR.SIENNA:
@ -153,26 +148,14 @@ class CarInterface(CarInterfaceBase):
ret.mass = 4590. * CV.LB_TO_KG + STD_CARGO_KG
set_lat_tune(ret.lateralTuning, LatTunes.PID_J)
elif candidate == CAR.LEXUS_IS:
ret.safetyConfigs[0].safetyParam = 77
stop_and_go = False
elif candidate in (CAR.LEXUS_IS, CAR.LEXUS_RC):
ret.wheelbase = 2.79908
ret.steerRatio = 13.3
tire_stiffness_factor = 0.444
ret.mass = 3736.8 * CV.LB_TO_KG + STD_CARGO_KG
set_lat_tune(ret.lateralTuning, LatTunes.PID_L)
elif candidate == CAR.LEXUS_RC:
ret.safetyConfigs[0].safetyParam = 77
stop_and_go = False
ret.wheelbase = 2.73050
ret.steerRatio = 13.3
tire_stiffness_factor = 0.444
ret.mass = 3736.8 * CV.LB_TO_KG + STD_CARGO_KG
set_lat_tune(ret.lateralTuning, LatTunes.PID_L)
elif candidate == CAR.LEXUS_CTH:
ret.safetyConfigs[0].safetyParam = 100
stop_and_go = True
ret.wheelbase = 2.60
ret.steerRatio = 18.6
@ -234,14 +217,13 @@ class CarInterface(CarInterfaceBase):
# if the smartDSU is detected, openpilot can send ACC_CMD (and the smartDSU will block it from the DSU) or not (the DSU is "connected")
ret.openpilotLongitudinalControl = smartDsu or ret.enableDsu or candidate in TSS2_CAR
if 0x245 in fingerprint[0]:
ret.flags |= ToyotaFlags.HYBRID.value
# min speed to enable ACC. if car can do stop and go, then set enabling speed
# to a negative value, so it won't matter.
ret.minEnableSpeed = -1. if (stop_and_go or ret.enableGasInterceptor) else MIN_ACC_SPEED
# removing the DSU disables AEB and it's considered a community maintained feature
# intercepting the DSU is a community feature since it requires unofficial hardware
ret.communityFeature = ret.enableGasInterceptor or ret.enableDsu or smartDsu
if ret.enableGasInterceptor:
set_long_tune(ret.longitudinalTuning, LongTunes.PEDAL)
elif candidate in TSS2_CAR:

8
selfdrive/car/toyota/radar_interface.py
Unescape View File

@ -4,6 +4,7 @@ from cereal import car
from selfdrive.car.toyota.values import NO_DSU_CAR, DBC, TSS2_CAR
from selfdrive.car.interfaces import RadarInterfaceBase
def _create_radar_can_parser(car_fingerprint):
if car_fingerprint in TSS2_CAR:
RADAR_A_MSGS = list(range(0x180, 0x190))
@ -16,11 +17,10 @@ def _create_radar_can_parser(car_fingerprint):
msg_b_n = len(RADAR_B_MSGS)
signals = list(zip(['LONG_DIST'] * msg_a_n + ['NEW_TRACK'] * msg_a_n + ['LAT_DIST'] * msg_a_n +
['REL_SPEED'] * msg_a_n + ['VALID'] * msg_a_n + ['SCORE'] * msg_b_n,
RADAR_A_MSGS * 5 + RADAR_B_MSGS,
[255] * msg_a_n + [1] * msg_a_n + [0] * msg_a_n + [0] * msg_a_n + [0] * msg_a_n + [0] * msg_b_n))
['REL_SPEED'] * msg_a_n + ['VALID'] * msg_a_n + ['SCORE'] * msg_b_n,
RADAR_A_MSGS * 5 + RADAR_B_MSGS))
checks = list(zip(RADAR_A_MSGS + RADAR_B_MSGS, [20]*(msg_a_n + msg_b_n)))
checks = list(zip(RADAR_A_MSGS + RADAR_B_MSGS, [20] * (msg_a_n + msg_b_n)))
return CANParser(DBC[car_fingerprint]['radar'], signals, checks, 1)

29
selfdrive/car/toyota/toyotacan.py
Unescape View File

@ -69,17 +69,32 @@ def create_fcw_command(packer, fcw):
def create_ui_command(packer, steer, chime, left_line, right_line, left_lane_depart, right_lane_depart, enabled):
values = {
"TWO_BEEPS": chime,
"LDA_ALERT": steer,
"RIGHT_LINE": 3 if right_lane_depart else 1 if right_line else 2,
"LEFT_LINE": 3 if left_lane_depart else 1 if left_line else 2,
"BARRIERS" : 1 if enabled else 0,
"SET_ME_X0C": 0x0c,
"SET_ME_X2C": 0x2c,
"SET_ME_X38": 0x38,
"SET_ME_X02": 0x02,
# static signals
"SET_ME_X02": 2,
"SET_ME_X01": 1,
"SET_ME_X01_2": 1,
"LKAS_STATUS": 1,
"REPEATED_BEEPS": 0,
"TWO_BEEPS": chime,
"LDA_ALERT": steer,
"LANE_SWAY_FLD": 7,
"LANE_SWAY_BUZZER": 0,
"LANE_SWAY_WARNING": 0,
"LDA_FRONT_CAMERA_BLOCKED": 0,
"TAKE_CONTROL": 0,
"LANE_SWAY_SENSITIVITY": 2,
"LANE_SWAY_TOGGLE": 1,
"LDA_ON_MESSAGE": 0,
"LDA_SPEED_TOO_LOW": 0,
"LDA_SA_TOGGLE": 1,
"LDA_SENSITIVITY": 2,
"LDA_UNAVAILABLE": 0,
"LDA_MALFUNCTION": 0,
"LDA_UNAVAILABLE_QUIET": 0,
"ADJUSTING_CAMERA": 0,
"LDW_EXIST": 1,
}
return packer.make_can_msg("LKAS_HUD", 0, values)

169
selfdrive/car/toyota/values.py
Unescape View File

@ -1,4 +1,5 @@
# flake8: noqa
from collections import defaultdict
from enum import IntFlag
from cereal import car
from selfdrive.car import dbc_dict
@ -18,12 +19,18 @@ class CarControllerParams:
STEER_DELTA_DOWN = 25 # always lower than 45 otherwise the Rav4 faults (Prius seems ok with 50)
STEER_ERROR_MAX = 350 # max delta between torque cmd and torque motor
class ToyotaFlags(IntFlag):
HYBRID = 1
class CAR:
# Toyota
ALPHARD_TSS2 = "TOYOTA ALPHARD 2020"
AVALON = "TOYOTA AVALON 2016"
AVALON_2019 = "TOYOTA AVALON 2019"
AVALONH_2019 = "TOYOTA AVALON HYBRID 2019"
AVALON_TSS2 = "TOYOTA AVALON 2022"
CAMRY = "TOYOTA CAMRY 2018"
CAMRYH = "TOYOTA CAMRY HYBRID 2018"
CAMRY_TSS2 = "TOYOTA CAMRY 2021" # TSS 2.5
@ -39,6 +46,7 @@ class CAR:
HIGHLANDERH = "TOYOTA HIGHLANDER HYBRID 2018"
HIGHLANDERH_TSS2 = "TOYOTA HIGHLANDER HYBRID 2020"
PRIUS = "TOYOTA PRIUS 2017"
PRIUS_V = "TOYOTA PRIUS v 2017"
PRIUS_TSS2 = "TOYOTA PRIUS TSS2 2021"
RAV4 = "TOYOTA RAV4 2017"
RAV4H = "TOYOTA RAV4 HYBRID 2017"
@ -66,25 +74,24 @@ class CAR:
STATIC_DSU_MSGS = [
(0x128, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.AVALON), 1, 3, b'\xf4\x01\x90\x83\x00\x37'),
(0x128, (CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ESH), 1, 3, b'\x03\x00\x20\x00\x00\x52'),
(0x141, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ESH, CAR.LEXUS_RX), 1, 2, b'\x00\x00\x00\x46'),
(0x160, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ESH, CAR.LEXUS_RX), 1, 7, b'\x00\x00\x08\x12\x01\x31\x9c\x51'),
(0x161, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.AVALON, CAR.LEXUS_RX), 1, 7, b'\x00\x1e\x00\x00\x00\x80\x07'),
(0x141, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ESH, CAR.LEXUS_RX, CAR.PRIUS_V), 1, 2, b'\x00\x00\x00\x46'),
(0x160, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ESH, CAR.LEXUS_RX, CAR.PRIUS_V), 1, 7, b'\x00\x00\x08\x12\x01\x31\x9c\x51'),
(0x161, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.AVALON, CAR.LEXUS_RX, CAR.PRIUS_V), 1, 7, b'\x00\x1e\x00\x00\x00\x80\x07'),
(0X161, (CAR.HIGHLANDERH, CAR.HIGHLANDER, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ESH), 1, 7, b'\x00\x1e\x00\xd4\x00\x00\x5b'),
(0x283, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ESH, CAR.LEXUS_RX), 0, 3, b'\x00\x00\x00\x00\x00\x00\x8c'),
(0x283, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ESH, CAR.LEXUS_RX, CAR.PRIUS_V), 0, 3, b'\x00\x00\x00\x00\x00\x00\x8c'),
(0x2E6, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH), 0, 3, b'\xff\xf8\x00\x08\x7f\xe0\x00\x4e'),
(0x2E7, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH), 0, 3, b'\xa8\x9c\x31\x9c\x00\x00\x00\x02'),
(0x33E, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH), 0, 20, b'\x0f\xff\x26\x40\x00\x1f\x00'),
(0x344, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ESH, CAR.LEXUS_RX), 0, 5, b'\x00\x00\x01\x00\x00\x00\x00\x50'),
(0x344, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ESH, CAR.LEXUS_RX, CAR.PRIUS_V), 0, 5, b'\x00\x00\x01\x00\x00\x00\x00\x50'),
(0x365, (CAR.PRIUS, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.HIGHLANDERH), 0, 20, b'\x00\x00\x00\x80\x03\x00\x08'),
(0x365, (CAR.RAV4, CAR.RAV4H, CAR.COROLLA, CAR.HIGHLANDER, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ESH, CAR.LEXUS_RX), 0, 20, b'\x00\x00\x00\x80\xfc\x00\x08'),
(0x365, (CAR.RAV4, CAR.RAV4H, CAR.COROLLA, CAR.HIGHLANDER, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ESH, CAR.LEXUS_RX, CAR.PRIUS_V), 0, 20, b'\x00\x00\x00\x80\xfc\x00\x08'),
(0x366, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.HIGHLANDERH), 0, 20, b'\x00\x00\x4d\x82\x40\x02\x00'),
(0x366, (CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ESH, CAR.LEXUS_RX), 0, 20, b'\x00\x72\x07\xff\x09\xfe\x00'),
(0x366, (CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ESH, CAR.LEXUS_RX, CAR.PRIUS_V), 0, 20, b'\x00\x72\x07\xff\x09\xfe\x00'),
(0x470, (CAR.PRIUS, CAR.LEXUS_RXH), 1, 100, b'\x00\x00\x02\x7a'),
(0x470, (CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.RAV4H, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ESH), 1, 100, b'\x00\x00\x01\x79'),
(0x4CB, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDERH, CAR.HIGHLANDER, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ESH, CAR.LEXUS_RX), 0, 100, b'\x0c\x00\x00\x00\x00\x00\x00\x00'),
(0x470, (CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.RAV4H, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ESH, CAR.PRIUS_V), 1, 100, b'\x00\x00\x01\x79'),
(0x4CB, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDERH, CAR.HIGHLANDER, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ESH, CAR.LEXUS_RX, CAR.PRIUS_V), 0, 100, b'\x0c\x00\x00\x00\x00\x00\x00\x00'),
]
FW_VERSIONS = {
CAR.AVALON: {
(Ecu.esp, 0x7b0, None): [
@ -166,6 +173,23 @@ FW_VERSIONS = {
b'8646F0702100\x00\x00\x00\x00',
],
},
CAR.AVALON_TSS2: {
(Ecu.esp, 0x7b0, None): [
b'\x01F152607280\x00\x00\x00\x00\x00\x00',
],
(Ecu.eps, 0x7a1, None): [
b'8965B41110\x00\x00\x00\x00\x00\x00',
],
(Ecu.engine, 0x700, None): [
b'\x01896630742000\x00\x00\x00\x00',
],
(Ecu.fwdRadar, 0x750, 0xf): [
b'\x018821F6201200\x00\x00\x00\x00',
],
(Ecu.fwdCamera, 0x750, 0x6d): [
b'\x028646F4104100\x00\x00\x00\x008646G5301200\x00\x00\x00\x00',
],
},
CAR.CAMRY: {
(Ecu.engine, 0x700, None): [
b'\x018966306L3100\x00\x00\x00\x00',
@ -344,12 +368,14 @@ FW_VERSIONS = {
b'\x018966306Q5000\x00\x00\x00\x00',
b'\x018966306T3100\x00\x00\x00\x00',
b'\x018966306T3200\x00\x00\x00\x00',
b'\x018966306T4000\x00\x00\x00\x00',
b'\x018966306T4100\x00\x00\x00\x00',
],
(Ecu.fwdRadar, 0x750, 0xf): [
b'\x018821F6201200\x00\x00\x00\x00',
],
(Ecu.fwdCamera, 0x750, 0x6d): [
b'\x028646F0602100\x00\x00\x00\x008646G5301200\x00\x00\x00\x00',
b'\x028646F0602200\x00\x00\x00\x008646G5301200\x00\x00\x00\x00',
b'\x028646F3305200\x00\x00\x00\x008646G5301200\x00\x00\x00\x00',
b'\x028646F3305300\x00\x00\x00\x008646G5301200\x00\x00\x00\x00',
@ -376,6 +402,7 @@ FW_VERSIONS = {
},
CAR.CHR: {
(Ecu.engine, 0x700, None): [
b'\x01896631021100\x00\x00\x00\x00',
b'\x01896631017100\x00\x00\x00\x00',
b'\x01896631017200\x00\x00\x00\x00',
b'\x0189663F413100\x00\x00\x00\x00',
@ -559,6 +586,7 @@ FW_VERSIONS = {
b'\x03312N6000\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00895231203402\x00\x00\x00\x00',
b'\x03312N6100\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00895231203302\x00\x00\x00\x00',
b'\x03312N6100\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00895231203402\x00\x00\x00\x00',
b'\x02312K4000\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.eps, 0x7a1, None): [
b'\x018965B12350\x00\x00\x00\x00\x00\x00',
@ -570,6 +598,7 @@ FW_VERSIONS = {
b'\x018965B1255000\x00\x00\x00\x00',
b'8965B12361\x00\x00\x00\x00\x00\x00',
b'8965B16011\x00\x00\x00\x00\x00\x00',
b'\x018965B12510\x00\x00\x00\x00\x00\x00'
],
(Ecu.esp, 0x7b0, None): [
b'\x01F152602280\x00\x00\x00\x00\x00\x00',
@ -584,11 +613,13 @@ FW_VERSIONS = {
b'\x01F152612B51\x00\x00\x00\x00\x00\x00',
b'\x01F152612B60\x00\x00\x00\x00\x00\x00',
b'\x01F152612B61\x00\x00\x00\x00\x00\x00',
b'\x01F152612B62\x00\x00\x00\x00\x00\x00',
b'\x01F152612B71\x00\x00\x00\x00\x00\x00',
b'\x01F152612B81\x00\x00\x00\x00\x00\x00',
b'\x01F152612B90\x00\x00\x00\x00\x00\x00',
b'\x01F152612C00\x00\x00\x00\x00\x00\x00',
b'F152602191\x00\x00\x00\x00\x00\x00',
b'\x01F152612862\x00\x00\x00\x00\x00\x00',
],
(Ecu.fwdRadar, 0x750, 0xf): [
b'\x018821F3301100\x00\x00\x00\x00',
@ -616,6 +647,7 @@ FW_VERSIONS = {
b'\x01896637624000\x00\x00\x00\x00',
b'\x01896637626000\x00\x00\x00\x00',
b'\x01896637648000\x00\x00\x00\x00',
b'\x01896637643000\x00\x00\x00\x00',
b'\x02896630ZJ5000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00',
b'\x02896630ZN8000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00',
b'\x02896630ZQ3000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00',
@ -792,9 +824,11 @@ FW_VERSIONS = {
b'\x01F15264872300\x00\x00\x00\x00',
b'\x01F15264872400\x00\x00\x00\x00',
b'\x01F15264872500\x00\x00\x00\x00',
b'\x01F15264873500\x00\x00\x00\x00',
b'\x01F152648C6300\x00\x00\x00\x00',
],
(Ecu.engine, 0x700, None): [
b'\x01896630E67000\x00\x00\x00\x00',
b'\x01896630EA1000\x00\x00\x00\x00',
b'\x01896630EE4000\x00\x00\x00\x00',
b'\x01896630EA1000\x00\x00\x00\x00897CF4801001\x00\x00\x00\x00',
@ -952,6 +986,23 @@ FW_VERSIONS = {
b'8646F4705200\x00\x00\x00\x00',
],
},
CAR.PRIUS_V: {
(Ecu.esp, 0x7b0, None): [
b'F152647280\x00\x00\x00\x00\x00\x00',
],
(Ecu.engine, 0x7e0, None): [
b'\x0234781000\x00\x00\x00\x00\x00\x00\x00\x00A4701000\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.dsu, 0x791, None): [
b'881514705100\x00\x00\x00\x00',
],
(Ecu.fwdRadar, 0x750, 0xf): [
b'8821F4702300\x00\x00\x00\x00',
],
(Ecu.fwdCamera, 0x750, 0x6d): [
b'8646F4703300\x00\x00\x00\x00',
],
},
CAR.RAV4: {
(Ecu.engine, 0x7e0, None): [
b'\x02342Q1000\x00\x00\x00\x00\x00\x00\x00\x0054212000\x00\x00\x00\x00\x00\x00\x00\x00',
@ -1504,20 +1555,21 @@ FW_VERSIONS = {
},
CAR.LEXUS_RX_TSS2: {
(Ecu.engine, 0x700, None): [
b'\x01896630EC9000\x00\x00\x00\x00',
b'\x01896634D12000\x00\x00\x00\x00',
b'\x01896630EB0000\x00\x00\x00\x00',
b'\x01896630EA9000\x00\x00\x00\x00',
b'\x01896630EB0000\x00\x00\x00\x00',
b'\x01896630EC9000\x00\x00\x00\x00',
b'\x01896630ED0000\x00\x00\x00\x00',
b'\x01896630ED6000\x00\x00\x00\x00',
b'\x018966348W5100\x00\x00\x00\x00',
b'\x018966348W9000\x00\x00\x00\x00',
b'\x01896634D12000\x00\x00\x00\x00',
b'\x01896634D12100\x00\x00\x00\x00',
],
(Ecu.esp, 0x7b0, None): [
b'\x01F152648801\x00\x00\x00\x00\x00\x00',
b'\x01F15260E031\x00\x00\x00\x00\x00\x00',
b'\x01F15260E041\x00\x00\x00\x00\x00\x00',
b'\x01F152648781\x00\x00\x00\x00\x00\x00',
b'\x01F152648801\x00\x00\x00\x00\x00\x00',
],
(Ecu.eps, 0x7a1, None): [
b'8965B48261\x00\x00\x00\x00\x00\x00',
@ -1529,8 +1581,9 @@ FW_VERSIONS = {
b'\x018821F3301400\x00\x00\x00\x00',
],
(Ecu.fwdCamera, 0x750, 0x6d): [
b'\x028646F4810200\x00\x00\x00\x008646G2601400\x00\x00\x00\x00',
b'\x028646F4810100\x00\x00\x00\x008646G2601200\x00\x00\x00\x00',
b'\x028646F4810200\x00\x00\x00\x008646G2601400\x00\x00\x00\x00',
b'\x028646F4810300\x00\x00\x00\x008646G2601400\x00\x00\x00\x00',
],
},
CAR.LEXUS_RXH_TSS2: {
@ -1591,63 +1644,79 @@ FW_VERSIONS = {
(Ecu.fwdCamera, 0x750, 0x6d): [b'\x028646F6201400\x00\x00\x00\x008646G5301200\x00\x00\x00\x00',],
},
CAR.ALPHARD_TSS2: {
(Ecu.engine, 0x7e0, None): [b'\x0235883000\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00',],
(Ecu.eps, 0x7a1, None): [b'8965B58040\x00\x00\x00\x00\x00\x00',],
(Ecu.fwdRadar, 0x750, 0xf): [b'\x018821F3301400\x00\x00\x00\x00',],
(Ecu.fwdCamera, 0x750, 0x6d): [b'\x028646F5803200\x00\x00\x00\x008646G2601400\x00\x00\x00\x00',],
(Ecu.engine, 0x7e0, None): [
b'\x0235870000\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00',
b'\x0235883000\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.eps, 0x7a1, None): [
b'8965B58040\x00\x00\x00\x00\x00\x00',
b'8965B58052\x00\x00\x00\x00\x00\x00',
],
(Ecu.fwdRadar, 0x750, 0xf): [
b'\x018821F3301200\x00\x00\x00\x00',
b'\x018821F3301400\x00\x00\x00\x00',
],
(Ecu.fwdCamera, 0x750, 0x6d): [
b'\x028646F58010C0\x00\x00\x00\x008646G26011A0\x00\x00\x00\x00',
b'\x028646F5803200\x00\x00\x00\x008646G2601400\x00\x00\x00\x00',
],
},
}
STEER_THRESHOLD = 100
DBC = {
CAR.RAV4H: dbc_dict('toyota_rav4_hybrid_2017_pt_generated', 'toyota_adas'),
CAR.RAV4: dbc_dict('toyota_rav4_2017_pt_generated', 'toyota_adas'),
CAR.PRIUS: dbc_dict('toyota_prius_2017_pt_generated', 'toyota_adas'),
CAR.COROLLA: dbc_dict('toyota_corolla_2017_pt_generated', 'toyota_adas'),
CAR.LEXUS_RC: dbc_dict('lexus_is_2018_pt_generated', 'toyota_adas'),
CAR.LEXUS_RX: dbc_dict('lexus_rx_350_2016_pt_generated', 'toyota_adas'),
CAR.LEXUS_RXH: dbc_dict('lexus_rx_hybrid_2017_pt_generated', 'toyota_adas'),
CAR.RAV4H: dbc_dict('toyota_tnga_k_pt_generated', 'toyota_adas'),
CAR.RAV4: dbc_dict('toyota_new_mc_pt_generated', 'toyota_adas'),
CAR.PRIUS: dbc_dict('toyota_nodsu_pt_generated', 'toyota_adas'),
CAR.PRIUS_V: dbc_dict('toyota_new_mc_pt_generated', 'toyota_adas'),
CAR.COROLLA: dbc_dict('toyota_new_mc_pt_generated', 'toyota_adas'),
CAR.LEXUS_RC: dbc_dict('toyota_tnga_k_pt_generated', 'toyota_adas'),
CAR.LEXUS_RX: dbc_dict('toyota_tnga_k_pt_generated', 'toyota_adas'),
CAR.LEXUS_RXH: dbc_dict('toyota_tnga_k_pt_generated', 'toyota_adas'),
CAR.LEXUS_RX_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'),
CAR.LEXUS_RXH_TSS2: dbc_dict('toyota_nodsu_hybrid_pt_generated', 'toyota_tss2_adas'),
CAR.LEXUS_RXH_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'),
CAR.CHR: dbc_dict('toyota_nodsu_pt_generated', 'toyota_adas'),
CAR.CHRH: dbc_dict('toyota_nodsu_hybrid_pt_generated', 'toyota_adas'),
CAR.CHRH: dbc_dict('toyota_nodsu_pt_generated', 'toyota_adas'),
CAR.CAMRY: dbc_dict('toyota_nodsu_pt_generated', 'toyota_adas'),
CAR.CAMRYH: dbc_dict('toyota_nodsu_hybrid_pt_generated', 'toyota_adas'),
CAR.CAMRYH: dbc_dict('toyota_nodsu_pt_generated', 'toyota_adas'),
CAR.CAMRY_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'),
CAR.CAMRYH_TSS2: dbc_dict('toyota_nodsu_hybrid_pt_generated', 'toyota_tss2_adas'),
CAR.HIGHLANDER: dbc_dict('toyota_highlander_2017_pt_generated', 'toyota_adas'),
CAR.CAMRYH_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'),
CAR.HIGHLANDER: dbc_dict('toyota_tnga_k_pt_generated', 'toyota_adas'),
CAR.HIGHLANDER_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'),
CAR.HIGHLANDERH: dbc_dict('toyota_highlander_hybrid_2018_pt_generated', 'toyota_adas'),
CAR.HIGHLANDERH_TSS2: dbc_dict('toyota_nodsu_hybrid_pt_generated', 'toyota_tss2_adas'),
CAR.AVALON: dbc_dict('toyota_avalon_2017_pt_generated', 'toyota_adas'),
CAR.HIGHLANDERH: dbc_dict('toyota_tnga_k_pt_generated', 'toyota_adas'),
CAR.HIGHLANDERH_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'),
CAR.AVALON: dbc_dict('toyota_tnga_k_pt_generated', 'toyota_adas'),
CAR.AVALON_2019: dbc_dict('toyota_nodsu_pt_generated', 'toyota_adas'),
CAR.AVALONH_2019: dbc_dict('toyota_nodsu_hybrid_pt_generated', 'toyota_adas'),
CAR.AVALONH_2019: dbc_dict('toyota_nodsu_pt_generated', 'toyota_adas'),
CAR.AVALON_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'),
CAR.RAV4_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'),
CAR.COROLLA_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'),
CAR.COROLLAH_TSS2: dbc_dict('toyota_nodsu_hybrid_pt_generated', 'toyota_tss2_adas'),
CAR.COROLLAH_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'),
CAR.LEXUS_ES_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'),
CAR.LEXUS_ESH_TSS2: dbc_dict('toyota_nodsu_hybrid_pt_generated', 'toyota_tss2_adas'),
CAR.LEXUS_ESH: dbc_dict('lexus_ct200h_2018_pt_generated', 'toyota_adas'),
CAR.SIENNA: dbc_dict('toyota_sienna_xle_2018_pt_generated', 'toyota_adas'),
CAR.LEXUS_IS: dbc_dict('lexus_is_2018_pt_generated', 'toyota_adas'),
CAR.LEXUS_CTH: dbc_dict('lexus_ct200h_2018_pt_generated', 'toyota_adas'),
CAR.RAV4H_TSS2: dbc_dict('toyota_nodsu_hybrid_pt_generated', 'toyota_tss2_adas'),
CAR.LEXUS_NXH: dbc_dict('lexus_nx300h_2018_pt_generated', 'toyota_adas'),
CAR.LEXUS_NX: dbc_dict('lexus_nx300_2018_pt_generated', 'toyota_adas'),
CAR.LEXUS_ESH_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'),
CAR.LEXUS_ESH: dbc_dict('toyota_new_mc_pt_generated', 'toyota_adas'),
CAR.SIENNA: dbc_dict('toyota_tnga_k_pt_generated', 'toyota_adas'),
CAR.LEXUS_IS: dbc_dict('toyota_tnga_k_pt_generated', 'toyota_adas'),
CAR.LEXUS_CTH: dbc_dict('toyota_new_mc_pt_generated', 'toyota_adas'),
CAR.RAV4H_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'),
CAR.LEXUS_NXH: dbc_dict('toyota_tnga_k_pt_generated', 'toyota_adas'),
CAR.LEXUS_NX: dbc_dict('toyota_tnga_k_pt_generated', 'toyota_adas'),
CAR.LEXUS_NX_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'),
CAR.PRIUS_TSS2: dbc_dict('toyota_nodsu_hybrid_pt_generated', 'toyota_tss2_adas'),
CAR.MIRAI: dbc_dict('toyota_nodsu_hybrid_pt_generated', 'toyota_tss2_adas'),
CAR.PRIUS_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'),
CAR.MIRAI: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'),
CAR.ALPHARD_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'),
}
# These cars have non-standard EPS torque scale factors. All others are 73
EPS_SCALE = defaultdict(lambda: 73, {CAR.PRIUS: 66, CAR.COROLLA: 88, CAR.LEXUS_IS: 77, CAR.LEXUS_RC: 77, CAR.LEXUS_CTH: 100, CAR.PRIUS_V: 100})
# Toyota/Lexus Safety Sense 2.0 and 2.5
TSS2_CAR = {CAR.RAV4_TSS2, CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2, CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2, CAR.RAV4H_TSS2,
CAR.LEXUS_RX_TSS2, CAR.LEXUS_RXH_TSS2, CAR.HIGHLANDER_TSS2, CAR.HIGHLANDERH_TSS2, CAR.PRIUS_TSS2, CAR.CAMRY_TSS2, CAR.CAMRYH_TSS2,
CAR.MIRAI, CAR.LEXUS_NX_TSS2, CAR.ALPHARD_TSS2}
CAR.LEXUS_RX_TSS2, CAR.LEXUS_RXH_TSS2, CAR.HIGHLANDER_TSS2, CAR.HIGHLANDERH_TSS2, CAR.PRIUS_TSS2, CAR.CAMRY_TSS2, CAR.CAMRYH_TSS2,
CAR.MIRAI, CAR.LEXUS_NX_TSS2, CAR.ALPHARD_TSS2, CAR.AVALON_TSS2}
NO_DSU_CAR = TSS2_CAR | {CAR.CHR, CAR.CHRH, CAR.CAMRY, CAR.CAMRYH}
# no resume button press required
NO_STOP_TIMER_CAR = TSS2_CAR | {CAR.RAV4H, CAR.HIGHLANDERH, CAR.HIGHLANDER, CAR.SIENNA, CAR.LEXUS_ESH}
NO_STOP_TIMER_CAR = TSS2_CAR | {CAR.PRIUS_V, CAR.RAV4H, CAR.HIGHLANDERH, CAR.HIGHLANDER, CAR.SIENNA, CAR.LEXUS_ESH}

4
selfdrive/car/volkswagen/carcontroller.py
Unescape View File

@ -26,7 +26,7 @@ class CarController():
self.acc_starting = False
self.acc_stopping = False
def update(self, enabled, CS, frame, ext_bus, actuators, visual_alert, left_lane_visible, right_lane_visible, left_lane_depart,
def update(self, c, enabled, CS, frame, ext_bus, actuators, visual_alert, left_lane_visible, right_lane_visible, left_lane_depart,
right_lane_depart, lead_visible, set_speed, speed_visible):
""" Controls thread """
@ -98,7 +98,7 @@ class CarController():
# torque value. Do that anytime we happen to have 0 torque, or failing that,
# when exceeding ~1/3 the 360 second timer.
if enabled and CS.out.vEgo > CS.CP.minSteerSpeed and not (CS.out.standstill or CS.out.steerError or CS.out.steerWarning):
if c.active and CS.out.vEgo > CS.CP.minSteerSpeed and not (CS.out.standstill or CS.out.steerError or CS.out.steerWarning):
new_steer = int(round(actuators.steer * P.STEER_MAX))
apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, P)
self.steer_rate_limited = new_steer != apply_steer

144
selfdrive/car/volkswagen/carstate.py
Unescape View File

@ -33,7 +33,7 @@ class CarState(CarStateBase):
# Update steering angle, rate, yaw rate, and driver input torque. VW send
# the sign/direction in a separate signal so they must be recombined.
ret.steeringAngleDeg = pt_cp.vl["LH_EPS_03"]["EPS_Berechneter_LW"] * (1, -1)[int(pt_cp.vl["LH_EPS_03"]["EPS_VZ_BLW"])]
ret.steeringAngleDeg = pt_cp.vl["LWI_01"]["LWI_Lenkradwinkel"] * (1, -1)[int(pt_cp.vl["LWI_01"]["LWI_VZ_Lenkradwinkel"])]
ret.steeringRateDeg = pt_cp.vl["LWI_01"]["LWI_Lenkradw_Geschw"] * (1, -1)[int(pt_cp.vl["LWI_01"]["LWI_VZ_Lenkradw_Geschw"])]
ret.steeringTorque = pt_cp.vl["LH_EPS_03"]["EPS_Lenkmoment"] * (1, -1)[int(pt_cp.vl["LH_EPS_03"]["EPS_VZ_Lenkmoment"])]
ret.steeringPressed = abs(ret.steeringTorque) > CarControllerParams.STEER_DRIVER_ALLOWANCE
@ -150,50 +150,49 @@ class CarState(CarStateBase):
@staticmethod
def get_can_parser(CP):
# this function generates lists for signal, messages and initial values
signals = [
# sig_name, sig_address, default
("EPS_Berechneter_LW", "LH_EPS_03", 0), # Absolute steering angle
("EPS_VZ_BLW", "LH_EPS_03", 0), # Steering angle sign
("LWI_Lenkradw_Geschw", "LWI_01", 0), # Absolute steering rate
("LWI_VZ_Lenkradw_Geschw", "LWI_01", 0), # Steering rate sign
("ESP_VL_Radgeschw_02", "ESP_19", 0), # ABS wheel speed, front left
("ESP_VR_Radgeschw_02", "ESP_19", 0), # ABS wheel speed, front right
("ESP_HL_Radgeschw_02", "ESP_19", 0), # ABS wheel speed, rear left
("ESP_HR_Radgeschw_02", "ESP_19", 0), # ABS wheel speed, rear right
("ESP_Gierrate", "ESP_02", 0), # Absolute yaw rate
("ESP_VZ_Gierrate", "ESP_02", 0), # Yaw rate sign
("ZV_FT_offen", "Gateway_72", 0), # Door open, driver
("ZV_BT_offen", "Gateway_72", 0), # Door open, passenger
("ZV_HFS_offen", "Gateway_72", 0), # Door open, rear left
("ZV_HBFS_offen", "Gateway_72", 0), # Door open, rear right
("ZV_HD_offen", "Gateway_72", 0), # Trunk or hatch open
("Comfort_Signal_Left", "Blinkmodi_02", 0), # Left turn signal including comfort blink interval
("Comfort_Signal_Right", "Blinkmodi_02", 0), # Right turn signal including comfort blink interval
("AB_Gurtschloss_FA", "Airbag_02", 0), # Seatbelt status, driver
("AB_Gurtschloss_BF", "Airbag_02", 0), # Seatbelt status, passenger
("ESP_Fahrer_bremst", "ESP_05", 0), # Brake pedal pressed
("ESP_Bremsdruck", "ESP_05", 0), # Brake pressure applied
("MO_Fahrpedalrohwert_01", "Motor_20", 0), # Accelerator pedal value
("EPS_Lenkmoment", "LH_EPS_03", 0), # Absolute driver torque input
("EPS_VZ_Lenkmoment", "LH_EPS_03", 0), # Driver torque input sign
("EPS_HCA_Status", "LH_EPS_03", 3), # EPS HCA control status
("ESP_Tastung_passiv", "ESP_21", 0), # Stability control disabled
("ESP_Haltebestaetigung", "ESP_21", 0), # ESP hold confirmation
("KBI_MFA_v_Einheit_02", "Einheiten_01", 0), # MPH vs KMH speed display
("KBI_Handbremse", "Kombi_01", 0), # Manual handbrake applied
("TSK_Status", "TSK_06", 0), # ACC engagement status from drivetrain coordinator
("GRA_Hauptschalter", "GRA_ACC_01", 0), # ACC button, on/off
("GRA_Abbrechen", "GRA_ACC_01", 0), # ACC button, cancel
("GRA_Tip_Setzen", "GRA_ACC_01", 0), # ACC button, set
("GRA_Tip_Hoch", "GRA_ACC_01", 0), # ACC button, increase or accel
("GRA_Tip_Runter", "GRA_ACC_01", 0), # ACC button, decrease or decel
("GRA_Tip_Wiederaufnahme", "GRA_ACC_01", 0), # ACC button, resume
("GRA_Verstellung_Zeitluecke", "GRA_ACC_01", 0), # ACC button, time gap adj
("GRA_Typ_Hauptschalter", "GRA_ACC_01", 0), # ACC main button type
("GRA_Tip_Stufe_2", "GRA_ACC_01", 0), # unknown related to stalk type
("GRA_ButtonTypeInfo", "GRA_ACC_01", 0), # unknown related to stalk type
("COUNTER", "GRA_ACC_01", 0), # GRA_ACC_01 CAN message counter
# sig_name, sig_address
("LWI_Lenkradwinkel", "LWI_01"), # Absolute steering angle
("LWI_VZ_Lenkradwinkel", "LWI_01"), # Steering angle sign
("LWI_Lenkradw_Geschw", "LWI_01"), # Absolute steering rate
("LWI_VZ_Lenkradw_Geschw", "LWI_01"), # Steering rate sign
("ESP_VL_Radgeschw_02", "ESP_19"), # ABS wheel speed, front left
("ESP_VR_Radgeschw_02", "ESP_19"), # ABS wheel speed, front right
("ESP_HL_Radgeschw_02", "ESP_19"), # ABS wheel speed, rear left
("ESP_HR_Radgeschw_02", "ESP_19"), # ABS wheel speed, rear right
("ESP_Gierrate", "ESP_02"), # Absolute yaw rate
("ESP_VZ_Gierrate", "ESP_02"), # Yaw rate sign
("ZV_FT_offen", "Gateway_72"), # Door open, driver
("ZV_BT_offen", "Gateway_72"), # Door open, passenger
("ZV_HFS_offen", "Gateway_72"), # Door open, rear left
("ZV_HBFS_offen", "Gateway_72"), # Door open, rear right
("ZV_HD_offen", "Gateway_72"), # Trunk or hatch open
("Comfort_Signal_Left", "Blinkmodi_02"), # Left turn signal including comfort blink interval
("Comfort_Signal_Right", "Blinkmodi_02"), # Right turn signal including comfort blink interval
("AB_Gurtschloss_FA", "Airbag_02"), # Seatbelt status, driver
("AB_Gurtschloss_BF", "Airbag_02"), # Seatbelt status, passenger
("ESP_Fahrer_bremst", "ESP_05"), # Brake pedal pressed
("ESP_Bremsdruck", "ESP_05"), # Brake pressure applied
("MO_Fahrpedalrohwert_01", "Motor_20"), # Accelerator pedal value
("EPS_Lenkmoment", "LH_EPS_03"), # Absolute driver torque input
("EPS_VZ_Lenkmoment", "LH_EPS_03"), # Driver torque input sign
("EPS_HCA_Status", "LH_EPS_03"), # EPS HCA control status
("ESP_Tastung_passiv", "ESP_21"), # Stability control disabled
("ESP_Haltebestaetigung", "ESP_21"), # ESP hold confirmation
("KBI_MFA_v_Einheit_02", "Einheiten_01"), # MPH vs KMH speed display
("KBI_Handbremse", "Kombi_01"), # Manual handbrake applied
("TSK_Status", "TSK_06"), # ACC engagement status from drivetrain coordinator
("GRA_Hauptschalter", "GRA_ACC_01"), # ACC button, on/off
("GRA_Abbrechen", "GRA_ACC_01"), # ACC button, cancel
("GRA_Tip_Setzen", "GRA_ACC_01"), # ACC button, set
("GRA_Tip_Hoch", "GRA_ACC_01"), # ACC button, increase or accel
("GRA_Tip_Runter", "GRA_ACC_01"), # ACC button, decrease or decel
("GRA_Tip_Wiederaufnahme", "GRA_ACC_01"), # ACC button, resume
("GRA_Verstellung_Zeitluecke", "GRA_ACC_01"), # ACC button, time gap adj
("GRA_Typ_Hauptschalter", "GRA_ACC_01"), # ACC main button type
("GRA_Tip_Stufe_2", "GRA_ACC_01"), # unknown related to stalk type
("GRA_ButtonTypeInfo", "GRA_ACC_01"), # unknown related to stalk type
("COUNTER", "GRA_ACC_01"), # GRA_ACC_01 CAN message counter
]
checks = [
@ -215,15 +214,15 @@ class CarState(CarStateBase):
]
if CP.transmissionType == TransmissionType.automatic:
signals += [("GE_Fahrstufe", "Getriebe_11", 0)] # Auto trans gear selector position
checks += [("Getriebe_11", 20)] # From J743 Auto transmission control module
signals.append(("GE_Fahrstufe", "Getriebe_11")) # Auto trans gear selector position
checks.append(("Getriebe_11", 20)) # From J743 Auto transmission control module
elif CP.transmissionType == TransmissionType.direct:
signals += [("GearPosition", "EV_Gearshift", 0)] # EV gear selector position
checks += [("EV_Gearshift", 10)] # From J??? unknown EV control module
signals.append(("GearPosition", "EV_Gearshift")) # EV gear selector position
checks.append(("EV_Gearshift", 10)) # From J??? unknown EV control module
elif CP.transmissionType == TransmissionType.manual:
signals += [("MO_Kuppl_schalter", "Motor_14", 0), # Clutch switch
("BCM1_Rueckfahrlicht_Schalter", "Gateway_72", 0)] # Reverse light from BCM
checks += [("Motor_14", 10)] # From J623 Engine control module
signals += [("MO_Kuppl_schalter", "Motor_14"), # Clutch switch
("BCM1_Rueckfahrlicht_Schalter", "Gateway_72")] # Reverse light from BCM
checks.append(("Motor_14", 10)) # From J623 Engine control module
if CP.networkLocation == NetworkLocation.fwdCamera:
# Radars are here on CANBUS.pt
@ -237,18 +236,17 @@ class CarState(CarStateBase):
@staticmethod
def get_cam_can_parser(CP):
signals = []
checks = []
if CP.networkLocation == NetworkLocation.fwdCamera:
signals += [
# sig_name, sig_address, default
("LDW_SW_Warnung_links", "LDW_02", 0), # Blind spot in warning mode on left side due to lane departure
("LDW_SW_Warnung_rechts", "LDW_02", 0), # Blind spot in warning mode on right side due to lane departure
("LDW_Seite_DLCTLC", "LDW_02", 0), # Direction of most likely lane departure (left or right)
("LDW_DLC", "LDW_02", 0), # Lane departure, distance to line crossing
("LDW_TLC", "LDW_02", 0), # Lane departure, time to line crossing
# sig_name, sig_address
("LDW_SW_Warnung_links", "LDW_02"), # Blind spot in warning mode on left side due to lane departure
("LDW_SW_Warnung_rechts", "LDW_02"), # Blind spot in warning mode on right side due to lane departure
("LDW_Seite_DLCTLC", "LDW_02"), # Direction of most likely lane departure (left or right)
("LDW_DLC", "LDW_02"), # Lane departure, distance to line crossing
("LDW_TLC", "LDW_02"), # Lane departure, time to line crossing
]
checks += [
# sig_address, frequency
@ -267,17 +265,17 @@ class CarState(CarStateBase):
class MqbExtraSignals:
# Additional signal and message lists for optional or bus-portable controllers
fwd_radar_signals = [
("ACC_Wunschgeschw", "ACC_02", 0), # ACC set speed
("ACC_Charisma_FahrPr", "ACC_04", 0), # Driving profile selection
("ACC_Charisma_Status", "ACC_04", 0), # Driving profile status
("ACC_Charisma_Umschaltung", "ACC_04", 0), # Driving profile switching
("ACC_Texte_braking_guard", "ACC_04", 0), # Part of ACC driver alerts in instrument cluster
("ACC_Typ", "ACC_06", 0), # Basic vs F2S vs SNG
("AWV2_Freigabe", "ACC_10", 0), # FCW brake jerk release
("ANB_Teilbremsung_Freigabe", "ACC_10", 0), # AEB partial braking release
("ANB_Zielbremsung_Freigabe", "ACC_10", 0), # AEB target braking release
("Unknown_Osc_1", "ACC_13", 0), # Unknown oscillating value (checksum/xor?)
("Unknown_Osc_2", "ACC_13", 0), # Unknown oscillating value (checksum/xor?)
("ACC_Wunschgeschw", "ACC_02"), # ACC set speed
("ACC_Charisma_FahrPr", "ACC_04"), # Driving profile selection
("ACC_Charisma_Status", "ACC_04"), # Driving profile status
("ACC_Charisma_Umschaltung", "ACC_04"), # Driving profile switching
("ACC_Texte_braking_guard", "ACC_04"), # Part of ACC driver alerts in instrument cluster
("ACC_Typ", "ACC_06"), # Basic vs F2S vs SNG
("AWV2_Freigabe", "ACC_10"), # FCW brake jerk release
("ANB_Teilbremsung_Freigabe", "ACC_10"), # AEB partial braking release
("ANB_Zielbremsung_Freigabe", "ACC_10"), # AEB target braking release
("Unknown_Osc_1", "ACC_13"), # Unknown oscillating value (checksum/xor?)
("Unknown_Osc_2", "ACC_13"), # Unknown oscillating value (checksum/xor?)
]
fwd_radar_checks = [
("ACC_06", 50), # From J428 ACC radar control module
@ -287,10 +285,10 @@ class MqbExtraSignals:
("ACC_13", 17), # From J428 ACC radar control module
]
bsm_radar_signals = [
("SWA_Infostufe_SWA_li", "SWA_01", 0), # Blind spot object info, left
("SWA_Warnung_SWA_li", "SWA_01", 0), # Blind spot object warning, left
("SWA_Infostufe_SWA_re", "SWA_01", 0), # Blind spot object info, right
("SWA_Warnung_SWA_re", "SWA_01", 0), # Blind spot object warning, right
("SWA_Infostufe_SWA_li", "SWA_01"), # Blind spot object info, left
("SWA_Warnung_SWA_li", "SWA_01"), # Blind spot object warning, left
("SWA_Infostufe_SWA_re", "SWA_01"), # Blind spot object info, right
("SWA_Warnung_SWA_re", "SWA_01"), # Blind spot object warning, right
]
bsm_radar_checks = [
("SWA_01", 20), # From J1086 Lane Change Assist

10
selfdrive/car/volkswagen/interface.py
Unescape View File

@ -54,7 +54,7 @@ class CarInterface(CarInterfaceBase):
# Global lateral tuning defaults, can be overridden per-vehicle
ret.steerActuatorDelay = 0.05
ret.steerActuatorDelay = 0.1
ret.steerRateCost = 1.0
ret.steerLimitTimer = 0.4
ret.steerRatio = 15.6 # Let the params learner figure this out
@ -194,12 +194,6 @@ class CarInterface(CarInterfaceBase):
ret.canValid = self.cp.can_valid and self.cp_cam.can_valid
ret.steeringRateLimited = self.CC.steer_rate_limited if self.CC is not None else False
# TODO: add a field for this to carState, car interface code shouldn't write params
# Update the device metric configuration to match the car at first startup,
# or if there's been a change.
#if self.CS.displayMetricUnits != self.displayMetricUnitsPrev:
# put_nonblocking("IsMetric", "1" if self.CS.displayMetricUnits else "0")
# Check for and process state-change events (button press or release) from
# the turn stalk switch or ACC steering wheel/control stalk buttons.
for button in self.CS.buttonStates:
@ -251,7 +245,7 @@ class CarInterface(CarInterfaceBase):
def apply(self, c):
hud_control = c.hudControl
ret = self.CC.update(c.enabled, self.CS, self.frame, self.ext_bus, c.actuators,
ret = self.CC.update(c, c.enabled, self.CS, self.frame, self.ext_bus, c.actuators,
hud_control.visualAlert,
hud_control.leftLaneVisible,
hud_control.rightLaneVisible,

9
selfdrive/car/volkswagen/values.py
Unescape View File

@ -1,5 +1,3 @@
# flake8: noqa
from collections import defaultdict
from typing import Dict
@ -129,6 +127,7 @@ FW_VERSIONS = {
CAR.ATLAS_MK1: {
(Ecu.engine, 0x7e0, None): [
b'\xf1\x8703H906026AA\xf1\x899970',
b'\xf1\x8703H906026AT\xf1\x891922',
b'\xf1\x8703H906026F \xf1\x896696',
b'\xf1\x8703H906026F \xf1\x899970',
b'\xf1\x8703H906026J \xf1\x896026',
@ -139,17 +138,21 @@ FW_VERSIONS = {
(Ecu.transmission, 0x7e1, None): [
b'\xf1\x8709G927158A \xf1\x893387',
b'\xf1\x8709G927158DR\xf1\x893536',
b'\xf1\x8709G927158FT\xf1\x893835',
],
(Ecu.srs, 0x715, None): [
b'\xf1\x873Q0959655BC\xf1\x890503\xf1\x82\0161914151912001103111122031200',
b'\xf1\x873Q0959655BN\xf1\x890713\xf1\x82\0162214152212001105141122052900',
b'\xf1\x873Q0959655DB\xf1\x890720\xf1\x82\0162214152212001105141122052900',
b'\xf1\x873Q0959655DM\xf1\x890732\xf1\x82\x0e1114151112001105161122052J00',
],
(Ecu.eps, 0x712, None): [
b'\xf1\x873QF909144B \xf1\x891582\xf1\x82\00571B60924A1',
b'\xf1\x873QF909144B \xf1\x891582\xf1\x82\x0571B6G920A1',
b'\xf1\x875Q0909143P \xf1\x892051\xf1\x820528B6090105',
],
(Ecu.fwdRadar, 0x757, None): [
b'\xf1\x872Q0907572T \xf1\x890383',
b'\xf1\x875Q0907572H \xf1\x890620',
b'\xf1\x875Q0907572J \xf1\x890654',
b'\xf1\x875Q0907572P \xf1\x890682',
@ -167,6 +170,7 @@ FW_VERSIONS = {
b'\xf1\x8704E906027HD\xf1\x893742',
b'\xf1\x8704E906027MA\xf1\x894958',
b'\xf1\x8704L906021DT\xf1\x895520',
b'\xf1\x8704L906021DT\xf1\x898127',
b'\xf1\x8704L906021N \xf1\x895518',
b'\xf1\x8704L906026BP\xf1\x897608',
b'\xf1\x8704L906026NF\xf1\x899528',
@ -213,6 +217,7 @@ FW_VERSIONS = {
b'\xf1\x870D9300040A \xf1\x893613',
b'\xf1\x870D9300040S \xf1\x894311',
b'\xf1\x870D9300041H \xf1\x895220',
b'\xf1\x870D9300041P \xf1\x894507',
b'\xf1\x870DD300045K \xf1\x891120',
b'\xf1\x870DD300046F \xf1\x891601',
b'\xf1\x870GC300012A \xf1\x891403',

2
selfdrive/common/SConscript
Unescape View File

@ -7,6 +7,7 @@ else:
common_libs = [
'params.cc',
'statlog.cc',
'swaglog.cc',
'util.cc',
'gpio.cc',
@ -33,3 +34,4 @@ Export('_common', '_gpucommon', '_gpu_libs')
if GetOption('test'):
env.Program('tests/test_util', ['tests/test_util.cc'], LIBS=[_common])
env.Program('tests/test_swaglog', ['tests/test_swaglog.cc'], LIBS=[_common, 'json11', 'zmq', 'pthread'])

14
selfdrive/common/params.cc
Unescape View File

@ -88,12 +88,11 @@ std::unordered_map<std::string, uint32_t> keys = {
{"AthenadUploadQueue", PERSISTENT},
{"CalibrationParams", PERSISTENT},
{"CarBatteryCapacity", PERSISTENT},
{"CarParams", CLEAR_ON_MANAGER_START | CLEAR_ON_PANDA_DISCONNECT | CLEAR_ON_IGNITION_ON},
{"CarParamsCache", CLEAR_ON_MANAGER_START | CLEAR_ON_PANDA_DISCONNECT},
{"CarVin", CLEAR_ON_MANAGER_START | CLEAR_ON_PANDA_DISCONNECT | CLEAR_ON_IGNITION_ON},
{"CommunityFeaturesToggle", PERSISTENT},
{"CarParams", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_ON},
{"CarParamsCache", CLEAR_ON_MANAGER_START},
{"CarVin", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_ON},
{"CompletedTrainingVersion", PERSISTENT},
{"ControlsReady", CLEAR_ON_MANAGER_START | CLEAR_ON_PANDA_DISCONNECT | CLEAR_ON_IGNITION_ON},
{"ControlsReady", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_ON},
{"CurrentRoute", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_ON},
{"DisablePowerDown", PERSISTENT},
{"DisableRadar_Allow", PERSISTENT},
@ -131,8 +130,10 @@ std::unordered_map<std::string, uint32_t> keys = {
{"JoystickDebugMode", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_OFF},
{"LastAthenaPingTime", CLEAR_ON_MANAGER_START},
{"LastGPSPosition", PERSISTENT},
{"LastManagerExitReason", CLEAR_ON_MANAGER_START},
{"LastPeripheralPandaType", PERSISTENT},
{"LastPowerDropDetected", CLEAR_ON_MANAGER_START},
{"LastSystemShutdown", CLEAR_ON_MANAGER_START},
{"LastUpdateException", PERSISTENT},
{"LastUpdateTime", PERSISTENT},
{"LiveParameters", PERSISTENT},
@ -141,6 +142,7 @@ std::unordered_map<std::string, uint32_t> keys = {
{"NavdRender", PERSISTENT},
{"OpenpilotEnabledToggle", PERSISTENT},
{"PandaHeartbeatLost", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_OFF},
{"PandaSignatures", CLEAR_ON_MANAGER_START},
{"Passive", PERSISTENT},
{"PrimeRedirected", PERSISTENT},
{"RecordFront", PERSISTENT},
@ -162,7 +164,7 @@ std::unordered_map<std::string, uint32_t> keys = {
{"ApiCache_NavDestinations", PERSISTENT},
{"ApiCache_Owner", PERSISTENT},
{"Offroad_CarUnrecognized", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_ON},
{"Offroad_ChargeDisabled", CLEAR_ON_MANAGER_START | CLEAR_ON_PANDA_DISCONNECT},
{"Offroad_ChargeDisabled", CLEAR_ON_MANAGER_START },
{"Offroad_ConnectivityNeeded", CLEAR_ON_MANAGER_START},
{"Offroad_ConnectivityNeededPrompt", CLEAR_ON_MANAGER_START},
{"Offroad_InvalidTime", CLEAR_ON_MANAGER_START},

7
selfdrive/common/params.h
Unescape View File

@ -6,10 +6,9 @@
enum ParamKeyType {
PERSISTENT = 0x02,
CLEAR_ON_MANAGER_START = 0x04,
CLEAR_ON_PANDA_DISCONNECT = 0x08,
CLEAR_ON_IGNITION_ON = 0x10,
CLEAR_ON_IGNITION_OFF = 0x20,
DONT_LOG = 0x40,
CLEAR_ON_IGNITION_ON = 0x08,
CLEAR_ON_IGNITION_OFF = 0x10,
DONT_LOG = 0x20,
ALL = 0xFFFFFFFF
};

43
selfdrive/common/statlog.cc
Unescape View File

@ -0,0 +1,43 @@
#ifndef _GNU_SOURCE
#define _GNU_SOURCE
#endif
#include "selfdrive/common/statlog.h"
#include "selfdrive/common/util.h"
#include <stdio.h>
#include <mutex>
#include <zmq.h>
class StatlogState : public LogState {
public:
StatlogState() : LogState("ipc:///tmp/stats") {}
};
static StatlogState s = {};
static void log(const char* metric_type, const char* metric, const char* fmt, ...) {
char* value_buf = nullptr;
va_list args;
va_start(args, fmt);
int ret = vasprintf(&value_buf, fmt, args);
va_end(args);
if (ret > 0 && value_buf) {
char* line_buf = nullptr;
ret = asprintf(&line_buf, "%s:%s|%s", metric, value_buf, metric_type);
if (ret > 0 && line_buf) {
zmq_send(s.sock, line_buf, ret, ZMQ_NOBLOCK);
free(line_buf);
}
free(value_buf);
}
}
void statlog_log(const char* metric_type, const char* metric, int value) {
log(metric_type, metric, "%d", value);
}
void statlog_log(const char* metric_type, const char* metric, float value) {
log(metric_type, metric, "%f", value);
}

10
selfdrive/common/statlog.h
Unescape View File

@ -0,0 +1,10 @@
#pragma once
#define STATLOG_GAUGE "g"
#define STATLOG_SAMPLE "sa"
void statlog_log(const char* metric_type, const char* metric, int value);
void statlog_log(const char* metric_type, const char* metric, float value);
#define statlog_gauge(metric, value) statlog_log(STATLOG_GAUGE, metric, value)
#define statlog_sample(metric, value) statlog_log(STATLOG_SAMPLE, metric, value)

114
selfdrive/common/swaglog.cc
Unescape View File

@ -16,75 +16,55 @@
#include "selfdrive/common/version.h"
#include "selfdrive/hardware/hw.h"
class LogState {
class SwaglogState : public LogState {
public:
LogState() = default;
~LogState();
std::mutex lock;
bool inited;
json11::Json::object ctx_j;
void *zctx;
void *sock;
int print_level;
};
SwaglogState() : LogState("ipc:///tmp/logmessage") {}
LogState::~LogState() {
zmq_close(sock);
zmq_ctx_destroy(zctx);
}
static LogState s = {};
bool initialized = false;
json11::Json::object ctx_j;
static void cloudlog_bind_locked(const char* k, const char* v) {
s.ctx_j[k] = v;
}
inline void initialize() {
ctx_j = json11::Json::object {};
print_level = CLOUDLOG_WARNING;
const char* print_lvl = getenv("LOGPRINT");
if (print_lvl) {
if (strcmp(print_lvl, "debug") == 0) {
print_level = CLOUDLOG_DEBUG;
} else if (strcmp(print_lvl, "info") == 0) {
print_level = CLOUDLOG_INFO;
} else if (strcmp(print_lvl, "warning") == 0) {
print_level = CLOUDLOG_WARNING;
}
}
static void cloudlog_init() {
if (s.inited) return;
s.ctx_j = json11::Json::object {};
s.zctx = zmq_ctx_new();
s.sock = zmq_socket(s.zctx, ZMQ_PUSH);
int timeout = 100; // 100 ms timeout on shutdown for messages to be received by logmessaged
zmq_setsockopt(s.sock, ZMQ_LINGER, &timeout, sizeof(timeout));
zmq_connect(s.sock, "ipc:///tmp/logmessage");
s.print_level = CLOUDLOG_WARNING;
const char* print_level = getenv("LOGPRINT");
if (print_level) {
if (strcmp(print_level, "debug") == 0) {
s.print_level = CLOUDLOG_DEBUG;
} else if (strcmp(print_level, "info") == 0) {
s.print_level = CLOUDLOG_INFO;
} else if (strcmp(print_level, "warning") == 0) {
s.print_level = CLOUDLOG_WARNING;
// openpilot bindings
char* dongle_id = getenv("DONGLE_ID");
if (dongle_id) {
ctx_j["dongle_id"] = dongle_id;
}
char* daemon_name = getenv("MANAGER_DAEMON");
if (daemon_name) {
ctx_j["daemon"] = daemon_name;
}
ctx_j["version"] = COMMA_VERSION;
ctx_j["dirty"] = !getenv("CLEAN");
// device type
if (Hardware::EON()) {
ctx_j["device"] = "eon";
} else if (Hardware::TICI()) {
ctx_j["device"] = "tici";
} else {
ctx_j["device"] = "pc";
}
}
// openpilot bindings
char* dongle_id = getenv("DONGLE_ID");
if (dongle_id) {
cloudlog_bind_locked("dongle_id", dongle_id);
}
cloudlog_bind_locked("version", COMMA_VERSION);
s.ctx_j["dirty"] = !getenv("CLEAN");
// device type
if (Hardware::EON()) {
cloudlog_bind_locked("device", "eon");
} else if (Hardware::TICI()) {
cloudlog_bind_locked("device", "tici");
} else {
cloudlog_bind_locked("device", "pc");
initialized = true;
}
};
s.inited = true;
}
static SwaglogState s = {};
void log(int levelnum, const char* filename, int lineno, const char* func, const char* msg, const std::string& log_s) {
std::lock_guard lk(s.lock);
cloudlog_init();
static void log(int levelnum, const char* filename, int lineno, const char* func, const char* msg, const std::string& log_s) {
if (levelnum >= s.print_level) {
printf("%s: %s\n", filename, msg);
}
@ -97,10 +77,14 @@ void cloudlog_e(int levelnum, const char* filename, int lineno, const char* func
char* msg_buf = nullptr;
va_list args;
va_start(args, fmt);
vasprintf(&msg_buf, fmt, args);
int ret = vasprintf(&msg_buf, fmt, args);
va_end(args);
if (!msg_buf) return;
if (ret <= 0 || !msg_buf) return;
std::lock_guard lk(s.lock);
if (!s.initialized) s.initialize();
json11::Json log_j = json11::Json::object {
{"msg", msg_buf},
@ -115,9 +99,3 @@ void cloudlog_e(int levelnum, const char* filename, int lineno, const char* func
log(levelnum, filename, lineno, func, msg_buf, log_s);
free(msg_buf);
}
void cloudlog_bind(const char* k, const char* v) {
std::lock_guard lk(s.lock);
cloudlog_init();
cloudlog_bind_locked(k, v);
}

4
selfdrive/common/swaglog.h
Unescape View File

@ -8,11 +8,11 @@
#define CLOUDLOG_ERROR 40
#define CLOUDLOG_CRITICAL 50
void cloudlog_e(int levelnum, const char* filename, int lineno, const char* func,
const char* fmt, ...) /*__attribute__ ((format (printf, 6, 7)))*/;
void cloudlog_bind(const char* k, const char* v);
#define cloudlog(lvl, fmt, ...) cloudlog_e(lvl, __FILE__, __LINE__, \
__func__, \
fmt, ## __VA_ARGS__)

1
selfdrive/common/tests/.gitignore vendored
Unescape View File

@ -1 +1,2 @@
test_util
test_swaglog

94
selfdrive/common/tests/test_swaglog.cc
Unescape View File

@ -0,0 +1,94 @@
#include <zmq.h>
#include <iostream>
#define CATCH_CONFIG_MAIN
#include "catch2/catch.hpp"
#include "json11.hpp"
#include "selfdrive/common/swaglog.h"
#include "selfdrive/common/util.h"
#include "selfdrive/common/version.h"
#include "selfdrive/hardware/hw.h"
const char *SWAGLOG_ADDR = "ipc:///tmp/logmessage";
std::string daemon_name = "testy";
std::string dongle_id = "test_dongle_id";
int LINE_NO = 0;
void log_thread(int thread_id, int msg_cnt) {
for (int i = 0; i < msg_cnt; ++i) {
LOGD("%d", thread_id);
LINE_NO = __LINE__ - 1;
usleep(1);
}
}
void recv_log(int thread_cnt, int thread_msg_cnt) {
void *zctx = zmq_ctx_new();
void *sock = zmq_socket(zctx, ZMQ_PULL);
zmq_bind(sock, SWAGLOG_ADDR);
std::vector<int> thread_msgs(thread_cnt);
int total_count = 0;
for (auto start = std::chrono::steady_clock::now(), now = start;
now < start + std::chrono::seconds{1} && total_count < (thread_cnt * thread_msg_cnt);
now = std::chrono::steady_clock::now()) {
char buf[4096] = {};
if (zmq_recv(sock, buf, sizeof(buf), ZMQ_DONTWAIT) <= 0) {
if (errno == EAGAIN || errno == EINTR || errno == EFSM) continue;
break;
}
REQUIRE(buf[0] == CLOUDLOG_DEBUG);
std::string err;
auto msg = json11::Json::parse(buf + 1, err);
REQUIRE(!msg.is_null());
REQUIRE(msg["levelnum"].int_value() == CLOUDLOG_DEBUG);
REQUIRE_THAT(msg["filename"].string_value(), Catch::Contains("test_swaglog.cc"));
REQUIRE(msg["funcname"].string_value() == "log_thread");
REQUIRE(msg["lineno"].int_value() == LINE_NO);
auto ctx = msg["ctx"];
REQUIRE(ctx["daemon"].string_value() == daemon_name);
REQUIRE(ctx["dongle_id"].string_value() == dongle_id);
REQUIRE(ctx["dirty"].bool_value() == true);
REQUIRE(ctx["version"].string_value() == COMMA_VERSION);
std::string device = "pc";
if (Hardware::EON()) {
device = "eon";
} else if (Hardware::TICI()) {
device = "tici";
}
REQUIRE(ctx["device"].string_value() == device);
int thread_id = atoi(msg["msg"].string_value().c_str());
REQUIRE((thread_id >= 0 && thread_id < thread_cnt));
thread_msgs[thread_id]++;
total_count++;
}
for (int i = 0; i < thread_cnt; ++i) {
INFO("thread :" << i);
REQUIRE(thread_msgs[i] == thread_msg_cnt);
}
zmq_close(sock);
zmq_ctx_destroy(zctx);
}
TEST_CASE("swaglog") {
setenv("MANAGER_DAEMON", daemon_name.c_str(), 1);
setenv("DONGLE_ID", dongle_id.c_str(), 1);
setenv("dirty", "1", 1);
const int thread_cnt = 5;
const int thread_msg_cnt = 100;
std::vector<std::thread> log_threads;
for (int i = 0; i < thread_cnt; ++i) {
log_threads.push_back(std::thread(log_thread, i, thread_msg_cnt));
}
for (auto &t : log_threads) t.join();
recv_log(thread_cnt, thread_msg_cnt);
}

29
selfdrive/common/util.h
Unescape View File

@ -3,6 +3,7 @@
#include <fcntl.h>
#include <sys/stat.h>
#include <unistd.h>
#include <zmq.h>
#include <algorithm>
#include <atomic>
@ -11,6 +12,7 @@
#include <ctime>
#include <map>
#include <memory>
#include <mutex>
#include <string>
#include <thread>
#include <vector>
@ -91,7 +93,9 @@ bool create_directories(const std::string &dir, mode_t mode);
std::string check_output(const std::string& command);
inline void sleep_for(const int milliseconds) {
std::this_thread::sleep_for(std::chrono::milliseconds(milliseconds));
if (milliseconds > 0) {
std::this_thread::sleep_for(std::chrono::milliseconds(milliseconds));
}
}
} // namespace util
@ -161,3 +165,26 @@ void update_max_atomic(std::atomic<T>& max, T const& value) {
T prev = max;
while(prev < value && !max.compare_exchange_weak(prev, value)) {}
}
class LogState {
public:
std::mutex lock;
void *zctx;
void *sock;
int print_level;
LogState(const char* endpoint) {
zctx = zmq_ctx_new();
sock = zmq_socket(zctx, ZMQ_PUSH);
// Timeout on shutdown for messages to be received by the logging process
int timeout = 100;
zmq_setsockopt(sock, ZMQ_LINGER, &timeout, sizeof(timeout));
zmq_connect(sock, endpoint);
};
~LogState() {
zmq_close(sock);
zmq_ctx_destroy(zctx);
}
};

90
selfdrive/controls/controlsd.py
Unescape View File

@ -31,15 +31,13 @@ from selfdrive.manager.process_config import managed_processes
SOFT_DISABLE_TIME = 3 # seconds
LDW_MIN_SPEED = 31 * CV.MPH_TO_MS
LANE_DEPARTURE_THRESHOLD = 0.1
STEER_ANGLE_SATURATION_TIMEOUT = 1.0 / DT_CTRL
STEER_ANGLE_SATURATION_THRESHOLD = 2.5 # Degrees
REPLAY = "REPLAY" in os.environ
SIMULATION = "SIMULATION" in os.environ
NOSENSOR = "NOSENSOR" in os.environ
IGNORE_PROCESSES = {"rtshield", "uploader", "deleter", "loggerd", "logmessaged", "tombstoned",
"logcatd", "proclogd", "clocksd", "updated", "timezoned", "manage_athenad",
"statsd"} | \
"statsd", "shutdownd"} | \
{k for k, v in managed_processes.items() if not v.enabled}
ACTUATOR_FIELDS = set(car.CarControl.Actuators.schema.fields.keys())
@ -55,7 +53,7 @@ ButtonEvent = car.CarState.ButtonEvent
SafetyModel = car.CarParams.SafetyModel
IGNORED_SAFETY_MODES = [SafetyModel.silent, SafetyModel.noOutput]
CSID_MAP = {"0": EventName.roadCameraError, "1": EventName.wideRoadCameraError, "2": EventName.driverCameraError}
class Controls:
def __init__(self, sm=None, pm=None, can_sock=None):
@ -100,7 +98,6 @@ class Controls:
# read params
self.is_metric = params.get_bool("IsMetric")
self.is_ldw_enabled = params.get_bool("IsLdwEnabled")
community_feature_toggle = params.get_bool("CommunityFeaturesToggle")
openpilot_enabled_toggle = params.get_bool("OpenpilotEnabledToggle")
passive = params.get_bool("Passive") or not openpilot_enabled_toggle
@ -110,11 +107,7 @@ class Controls:
car_recognized = self.CP.carName != 'mock'
controller_available = self.CI.CC is not None and not passive and not self.CP.dashcamOnly
community_feature = self.CP.communityFeature or \
self.CP.fingerprintSource == car.CarParams.FingerprintSource.can
community_feature_disallowed = community_feature and (not community_feature_toggle)
self.read_only = not car_recognized or not controller_available or \
self.CP.dashcamOnly or community_feature_disallowed
self.read_only = not car_recognized or not controller_available or self.CP.dashcamOnly
if self.read_only:
safety_config = car.CarParams.SafetyConfig.new_message()
safety_config.safetyModel = car.CarParams.SafetyModel.noOutput
@ -133,13 +126,13 @@ class Controls:
self.VM = VehicleModel(self.CP)
if self.CP.steerControlType == car.CarParams.SteerControlType.angle:
self.LaC = LatControlAngle(self.CP)
self.LaC = LatControlAngle(self.CP, self.CI)
elif self.CP.lateralTuning.which() == 'pid':
self.LaC = LatControlPID(self.CP, self.CI)
elif self.CP.lateralTuning.which() == 'indi':
self.LaC = LatControlINDI(self.CP)
self.LaC = LatControlINDI(self.CP, self.CI)
elif self.CP.lateralTuning.which() == 'lqr':
self.LaC = LatControlLQR(self.CP)
self.LaC = LatControlLQR(self.CP, self.CI)
self.initialized = False
self.state = State.disabled
@ -153,7 +146,6 @@ class Controls:
self.cruise_mismatch_counter = 0
self.can_rcv_error_counter = 0
self.last_blinker_frame = 0
self.saturated_count = 0
self.distance_traveled = 0
self.last_functional_fan_frame = 0
self.events_prev = []
@ -169,8 +161,6 @@ class Controls:
if not sounds_available:
self.events.add(EventName.soundsUnavailable, static=True)
if community_feature_disallowed and car_recognized and not self.CP.dashcamOnly:
self.events.add(EventName.communityFeatureDisallowed, static=True)
if not car_recognized:
self.events.add(EventName.carUnrecognized, static=True)
if len(self.CP.carFw) > 0:
@ -191,10 +181,8 @@ class Controls:
"""Compute carEvents from carState"""
self.events.clear()
self.events.add_from_msg(CS.events)
self.events.add_from_msg(self.sm['driverMonitoringState'].events)
# Handle startup event
# Add startup event
if self.startup_event is not None:
self.events.add(self.startup_event)
self.startup_event = None
@ -204,6 +192,9 @@ class Controls:
self.events.add(EventName.controlsInitializing)
return
self.events.add_from_msg(CS.events)
self.events.add_from_msg(self.sm['driverMonitoringState'].events)
# Create events for battery, temperature, disk space, and memory
if EON and (self.sm['peripheralState'].pandaType != PandaType.uno) and \
self.sm['deviceState'].batteryPercent < 1 and self.sm['deviceState'].chargingError:
@ -260,9 +251,12 @@ class Controls:
for i, pandaState in enumerate(self.sm['pandaStates']):
# All pandas must match the list of safetyConfigs, and if outside this list, must be silent or noOutput
if i < len(self.CP.safetyConfigs):
safety_mismatch = pandaState.safetyModel != self.CP.safetyConfigs[i].safetyModel or pandaState.safetyParam != self.CP.safetyConfigs[i].safetyParam
safety_mismatch = pandaState.safetyModel != self.CP.safetyConfigs[i].safetyModel or \
pandaState.safetyParam != self.CP.safetyConfigs[i].safetyParam or \
pandaState.unsafeMode != self.CP.unsafeMode
else:
safety_mismatch = pandaState.safetyModel not in IGNORED_SAFETY_MODES
if safety_mismatch or self.mismatch_counter >= 200:
self.events.add(EventName.controlsMismatch)
@ -279,7 +273,7 @@ class Controls:
if not self.logged_comm_issue:
invalid = [s for s, valid in self.sm.valid.items() if not valid]
not_alive = [s for s, alive in self.sm.alive.items() if not alive]
cloudlog.event("commIssue", invalid=invalid, not_alive=not_alive)
cloudlog.event("commIssue", invalid=invalid, not_alive=not_alive, can_error=self.can_rcv_error, error=True)
self.logged_comm_issue = True
else:
self.logged_comm_issue = False
@ -311,25 +305,17 @@ class Controls:
self.events.add(EventName.fcw)
if TICI:
logs = messaging.drain_sock(self.log_sock, wait_for_one=False)
messages = []
for m in logs:
for m in messaging.drain_sock(self.log_sock, wait_for_one=False):
try:
messages.append(m.androidLog.message)
msg = m.androidLog.message
if any(err in msg for err in ("ERROR_CRC", "ERROR_ECC", "ERROR_STREAM_UNDERFLOW", "APPLY FAILED")):
csid = msg.split("CSID:")[-1].split(" ")[0]
evt = CSID_MAP.get(csid, None)
if evt is not None:
self.events.add(evt)
except UnicodeDecodeError:
pass
for err in ("ERROR_CRC", "ERROR_ECC", "ERROR_STREAM_UNDERFLOW", "APPLY FAILED"):
for m in messages:
if err not in m:
continue
csid = m.split("CSID:")[-1].split(" ")[0]
evt = {"0": EventName.roadCameraError, "1": EventName.wideRoadCameraError,
"2": EventName.driverCameraError}.get(csid, None)
if evt is not None:
self.events.add(evt)
# TODO: fix simulator
if not SIMULATION:
if not NOSENSOR:
@ -373,6 +359,10 @@ class Controls:
if not self.read_only:
self.CI.init(self.CP, self.can_sock, self.pm.sock['sendcan'])
self.initialized = True
if REPLAY and self.sm['pandaStates'][0].controlsAllowed:
self.state = State.enabled
Params().put_bool("ControlsReady", True)
# Check for CAN timeout
@ -390,7 +380,7 @@ class Controls:
self.mismatch_counter = 0
# All pandas not in silent mode must have controlsAllowed when openpilot is enabled
if any(not ps.controlsAllowed and self.enabled for ps in self.sm['pandaStates']
if self.enabled and any(not ps.controlsAllowed for ps in self.sm['pandaStates']
if ps.safetyModel not in IGNORED_SAFETY_MODES):
self.mismatch_counter += 1
@ -440,7 +430,7 @@ class Controls:
# no more soft disabling condition, so go back to ENABLED
self.state = State.enabled
elif self.events.any(ET.SOFT_DISABLE) and self.soft_disable_timer > 0:
elif self.soft_disable_timer > 0:
self.current_alert_types.append(ET.SOFT_DISABLE)
elif self.soft_disable_timer <= 0:
@ -526,19 +516,8 @@ class Controls:
lac_log.output = steer
lac_log.saturated = abs(steer) >= 0.9
# Check for difference between desired angle and angle for angle based control
angle_control_saturated = self.CP.steerControlType == car.CarParams.SteerControlType.angle and \
abs(actuators.steeringAngleDeg - CS.steeringAngleDeg) > STEER_ANGLE_SATURATION_THRESHOLD
if angle_control_saturated and not CS.steeringPressed and self.active:
self.saturated_count += 1
else:
self.saturated_count = 0
# Send a "steering required alert" if saturation count has reached the limit
if (lac_log.saturated and not CS.steeringPressed) or \
(self.saturated_count > STEER_ANGLE_SATURATION_TIMEOUT):
if lac_log.active and lac_log.saturated and not CS.steeringPressed:
dpath_points = lat_plan.dPathPoints
if len(dpath_points):
# Check if we deviated from the path
@ -563,7 +542,7 @@ class Controls:
def update_button_timers(self, buttonEvents):
# increment timer for buttons still pressed
for k in self.button_timers.keys():
for k in self.button_timers:
if self.button_timers[k] > 0:
self.button_timers[k] += 1
@ -619,10 +598,15 @@ class Controls:
if hudControl.rightLaneDepart or hudControl.leftLaneDepart:
self.events.add(EventName.ldw)
clear_event = ET.WARNING if ET.WARNING not in self.current_alert_types else None
clear_event_types = set()
if ET.WARNING not in self.current_alert_types:
clear_event_types.add(ET.WARNING)
if self.enabled:
clear_event_types.add(ET.NO_ENTRY)
alerts = self.events.create_alerts(self.current_alert_types, [self.CP, self.sm, self.is_metric, self.soft_disable_timer])
self.AM.add_many(self.sm.frame, alerts)
current_alert = self.AM.process_alerts(self.sm.frame, clear_event)
current_alert = self.AM.process_alerts(self.sm.frame, clear_event_types)
if current_alert:
hudControl.visualAlert = current_alert.visual_alert

16
selfdrive/controls/lib/alertmanager.py
Unescape View File

@ -40,20 +40,20 @@ class AlertManager:
def add_many(self, frame: int, alerts: List[Alert]) -> None:
for alert in alerts:
key = alert.alert_type
self.alerts[key].alert = alert
if not self.alerts[key].active(frame):
self.alerts[key].start_frame = frame
min_end_frame = self.alerts[key].start_frame + alert.duration
self.alerts[key].end_frame = max(frame + 1, min_end_frame)
entry = self.alerts[alert.alert_type]
entry.alert = alert
if not entry.active(frame):
entry.start_frame = frame
min_end_frame = entry.start_frame + alert.duration
entry.end_frame = max(frame + 1, min_end_frame)
def process_alerts(self, frame: int, clear_event_type=None) -> Optional[Alert]:
def process_alerts(self, frame: int, clear_event_types: set) -> Optional[Alert]:
current_alert = AlertEntry()
for v in self.alerts.values():
if not v.alert:
continue
if clear_event_type and v.alert.event_type == clear_event_type:
if v.alert.event_type in clear_event_types:
v.end_frame = -1
# sort by priority first and then by start_frame

2
selfdrive/controls/lib/alerts_offroad.json
Unescape View File

@ -38,7 +38,7 @@
"severity": 1
},
"Offroad_StorageMissing": {
"text": "Storage drive not mounted.",
"text": "NVMe drive not mounted.",
"severity": 1
},
"Offroad_CarUnrecognized": {

113
selfdrive/controls/lib/desire_helper.py
Unescape View File

@ -0,0 +1,113 @@
from cereal import log
from common.realtime import DT_MDL
from selfdrive.config import Conversions as CV
LaneChangeState = log.LateralPlan.LaneChangeState
LaneChangeDirection = log.LateralPlan.LaneChangeDirection
LANE_CHANGE_SPEED_MIN = 30 * CV.MPH_TO_MS
LANE_CHANGE_TIME_MAX = 10.
DESIRES = {
LaneChangeDirection.none: {
LaneChangeState.off: log.LateralPlan.Desire.none,
LaneChangeState.preLaneChange: log.LateralPlan.Desire.none,
LaneChangeState.laneChangeStarting: log.LateralPlan.Desire.none,
LaneChangeState.laneChangeFinishing: log.LateralPlan.Desire.none,
},
LaneChangeDirection.left: {
LaneChangeState.off: log.LateralPlan.Desire.none,
LaneChangeState.preLaneChange: log.LateralPlan.Desire.none,
LaneChangeState.laneChangeStarting: log.LateralPlan.Desire.laneChangeLeft,
LaneChangeState.laneChangeFinishing: log.LateralPlan.Desire.laneChangeLeft,
},
LaneChangeDirection.right: {
LaneChangeState.off: log.LateralPlan.Desire.none,
LaneChangeState.preLaneChange: log.LateralPlan.Desire.none,
LaneChangeState.laneChangeStarting: log.LateralPlan.Desire.laneChangeRight,
LaneChangeState.laneChangeFinishing: log.LateralPlan.Desire.laneChangeRight,
},
}
class DesireHelper:
def __init__(self):
self.lane_change_state = LaneChangeState.off
self.lane_change_direction = LaneChangeDirection.none
self.lane_change_timer = 0.0
self.lane_change_ll_prob = 1.0
self.keep_pulse_timer = 0.0
self.prev_one_blinker = False
self.desire = log.LateralPlan.Desire.none
def update(self, carstate, active, lane_change_prob):
v_ego = carstate.vEgo
one_blinker = carstate.leftBlinker != carstate.rightBlinker
below_lane_change_speed = v_ego < LANE_CHANGE_SPEED_MIN
if not active or self.lane_change_timer > LANE_CHANGE_TIME_MAX:
self.lane_change_state = LaneChangeState.off
self.lane_change_direction = LaneChangeDirection.none
else:
# LaneChangeState.off
if self.lane_change_state == LaneChangeState.off and one_blinker and not self.prev_one_blinker and not below_lane_change_speed:
self.lane_change_state = LaneChangeState.preLaneChange
self.lane_change_ll_prob = 1.0
# LaneChangeState.preLaneChange
elif self.lane_change_state == LaneChangeState.preLaneChange:
# Set lane change direction
self.lane_change_direction = LaneChangeDirection.left if \
carstate.leftBlinker else LaneChangeDirection.right
torque_applied = carstate.steeringPressed and \
((carstate.steeringTorque > 0 and self.lane_change_direction == LaneChangeDirection.left) or
(carstate.steeringTorque < 0 and self.lane_change_direction == LaneChangeDirection.right))
blindspot_detected = ((carstate.leftBlindspot and self.lane_change_direction == LaneChangeDirection.left) or
(carstate.rightBlindspot and self.lane_change_direction == LaneChangeDirection.right))
if not one_blinker or below_lane_change_speed:
self.lane_change_state = LaneChangeState.off
elif torque_applied and not blindspot_detected:
self.lane_change_state = LaneChangeState.laneChangeStarting
# LaneChangeState.laneChangeStarting
elif self.lane_change_state == LaneChangeState.laneChangeStarting:
# fade out over .5s
self.lane_change_ll_prob = max(self.lane_change_ll_prob - 2 * DT_MDL, 0.0)
# 98% certainty
if lane_change_prob < 0.02 and self.lane_change_ll_prob < 0.01:
self.lane_change_state = LaneChangeState.laneChangeFinishing
# LaneChangeState.laneChangeFinishing
elif self.lane_change_state == LaneChangeState.laneChangeFinishing:
# fade in laneline over 1s
self.lane_change_ll_prob = min(self.lane_change_ll_prob + DT_MDL, 1.0)
if self.lane_change_ll_prob > 0.99:
self.lane_change_direction = LaneChangeDirection.none
if one_blinker:
self.lane_change_state = LaneChangeState.preLaneChange
else:
self.lane_change_state = LaneChangeState.off
if self.lane_change_state in (LaneChangeState.off, LaneChangeState.preLaneChange):
self.lane_change_timer = 0.0
else:
self.lane_change_timer += DT_MDL
self.prev_one_blinker = one_blinker
self.desire = DESIRES[self.lane_change_direction][self.lane_change_state]
# Send keep pulse once per second during LaneChangeStart.preLaneChange
if self.lane_change_state in (LaneChangeState.off, LaneChangeState.laneChangeStarting):
self.keep_pulse_timer = 0.0
elif self.lane_change_state == LaneChangeState.preLaneChange:
self.keep_pulse_timer += DT_MDL
if self.keep_pulse_timer > 1.0:
self.keep_pulse_timer = 0.0
elif self.desire in (log.LateralPlan.Desire.keepLeft, log.LateralPlan.Desire.keepRight):
self.desire = log.LateralPlan.Desire.none

17
selfdrive/controls/lib/events.py
Unescape View File

@ -93,7 +93,7 @@ class Events:
for event_name in self.events:
event = car.CarEvent.new_message()
event.name = event_name
for event_type in EVENTS.get(event_name, {}).keys():
for event_type in EVENTS.get(event_name, {}):
setattr(event, event_type, True)
ret.append(event)
return ret
@ -264,8 +264,6 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = {
EventName.stockFcw: {},
EventName.lkasDisabled: {},
# ********** events only containing alerts displayed in all states **********
EventName.joystickDebug: {
@ -316,14 +314,6 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = {
#ET.PERMANENT: ImmediateDisableAlert("openpilot failed to cancel cruise"),
},
# Some features or cars are marked as community features. If openpilot
# detects the use of a community feature it switches to dashcam mode
# until these features are allowed using a toggle in settings.
EventName.communityFeatureDisallowed: {
ET.PERMANENT: NormalPermanentAlert("openpilot Unavailable",
"Enable Community Features in Settings"),
},
# openpilot doesn't recognize the car. This switches openpilot into a
# read-only mode. This can be solved by adding your fingerprint.
# See https://github.com/commaai/openpilot/wiki/Fingerprinting for more information
@ -831,4 +821,9 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = {
ET.NO_ENTRY: NoEntryAlert("Cruise Fault: Restart the Car"),
},
EventName.lkasDisabled: {
ET.PERMANENT: NormalPermanentAlert("LKAS Disabled: Enable LKAS to engage"),
ET.NO_ENTRY: NoEntryAlert("LKAS Disabled"),
},
}

28
selfdrive/controls/lib/latcontrol.py
Unescape View File

@ -0,0 +1,28 @@
from abc import abstractmethod, ABC
from common.realtime import DT_CTRL
from common.numpy_fast import clip
MIN_STEER_SPEED = 0.3
class LatControl(ABC):
def __init__(self, CP, CI):
self.sat_count_rate = 1.0 * DT_CTRL
self.sat_limit = CP.steerLimitTimer
self.sat_count = 0.
@abstractmethod
def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate):
pass
def reset(self):
self.sat_count = 0.
def _check_saturation(self, saturated, CS):
if saturated and CS.vEgo > 10. and not CS.steeringRateLimited and not CS.steeringPressed:
self.sat_count += self.sat_count_rate
else:
self.sat_count -= self.sat_count_rate
self.sat_count = clip(self.sat_count, 0.0, self.sat_limit)
return self.sat_count > (self.sat_limit - 1e-3)

15
selfdrive/controls/lib/latcontrol_angle.py
Unescape View File

@ -1,19 +1,16 @@
import math
from cereal import log
from selfdrive.controls.lib.latcontrol import LatControl, MIN_STEER_SPEED
STEER_ANGLE_SATURATION_THRESHOLD = 2.5 # Degrees
class LatControlAngle():
def __init__(self, CP):
pass
def reset(self):
pass
class LatControlAngle(LatControl):
def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate):
angle_log = log.ControlsState.LateralAngleState.new_message()
if CS.vEgo < 0.3 or not active:
if CS.vEgo < MIN_STEER_SPEED or not active:
angle_log.active = False
angle_steers_des = float(CS.steeringAngleDeg)
else:
@ -21,8 +18,8 @@ class LatControlAngle():
angle_steers_des = math.degrees(VM.get_steer_from_curvature(-desired_curvature, CS.vEgo, params.roll))
angle_steers_des += params.angleOffsetDeg
angle_log.saturated = False
angle_control_saturated = abs(angle_steers_des - CS.steeringAngleDeg) > STEER_ANGLE_SATURATION_THRESHOLD
angle_log.saturated = self._check_saturation(angle_control_saturated, CS)
angle_log.steeringAngleDeg = float(CS.steeringAngleDeg)
angle_log.steeringAngleDesiredDeg = angle_steers_des
return 0, float(angle_steers_des), angle_log

61
selfdrive/controls/lib/latcontrol_indi.py
Unescape View File

@ -5,13 +5,13 @@ from cereal import log
from common.filter_simple import FirstOrderFilter
from common.numpy_fast import clip, interp
from common.realtime import DT_CTRL
from selfdrive.car import apply_toyota_steer_torque_limits
from selfdrive.car.toyota.values import CarControllerParams
from selfdrive.controls.lib.drive_helpers import get_steer_max
from selfdrive.controls.lib.latcontrol import LatControl, MIN_STEER_SPEED
class LatControlINDI():
def __init__(self, CP):
class LatControlINDI(LatControl):
def __init__(self, CP, CI):
super().__init__(CP, CI)
self.angle_steers_des = 0.
A = np.array([[1.0, DT_CTRL, 0.0],
@ -35,15 +35,11 @@ class LatControlINDI():
self.A_K = A - np.dot(K, C)
self.x = np.array([[0.], [0.], [0.]])
self.enforce_rate_limit = CP.carName == "toyota"
self._RC = (CP.lateralTuning.indi.timeConstantBP, CP.lateralTuning.indi.timeConstantV)
self._G = (CP.lateralTuning.indi.actuatorEffectivenessBP, CP.lateralTuning.indi.actuatorEffectivenessV)
self._outer_loop_gain = (CP.lateralTuning.indi.outerLoopGainBP, CP.lateralTuning.indi.outerLoopGainV)
self._inner_loop_gain = (CP.lateralTuning.indi.innerLoopGainBP, CP.lateralTuning.indi.innerLoopGainV)
self.sat_count_rate = 1.0 * DT_CTRL
self.sat_limit = CP.steerLimitTimer
self.steer_filter = FirstOrderFilter(0., self.RC, DT_CTRL)
self.reset()
@ -65,24 +61,11 @@ class LatControlINDI():
return interp(self.speed, self._inner_loop_gain[0], self._inner_loop_gain[1])
def reset(self):
super().reset()
self.steer_filter.x = 0.
self.output_steer = 0.
self.sat_count = 0.
self.speed = 0.
def _check_saturation(self, control, check_saturation, limit):
saturated = abs(control) == limit
if saturated and check_saturation:
self.sat_count += self.sat_count_rate
else:
self.sat_count -= self.sat_count_rate
self.sat_count = clip(self.sat_count, 0.0, 1.0)
return self.sat_count > self.sat_limit
def update(self, active, CS, CP, VM, params, last_actuators, curvature, curvature_rate):
def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate):
self.speed = CS.vEgo
# Update Kalman filter
y = np.array([[math.radians(CS.steeringAngleDeg)], [math.radians(CS.steeringRateDeg)]])
@ -93,21 +76,21 @@ class LatControlINDI():
indi_log.steeringRateDeg = math.degrees(self.x[1])
indi_log.steeringAccelDeg = math.degrees(self.x[2])
steers_des = VM.get_steer_from_curvature(-curvature, CS.vEgo, params.roll)
steers_des = VM.get_steer_from_curvature(-desired_curvature, CS.vEgo, params.roll)
steers_des += math.radians(params.angleOffsetDeg)
indi_log.steeringAngleDesiredDeg = math.degrees(steers_des)
rate_des = VM.get_steer_from_curvature(-curvature_rate, CS.vEgo, 0)
rate_des = VM.get_steer_from_curvature(-desired_curvature_rate, CS.vEgo, 0)
indi_log.steeringRateDesiredDeg = math.degrees(rate_des)
if CS.vEgo < 0.3 or not active:
if CS.vEgo < MIN_STEER_SPEED or not active:
indi_log.active = False
self.output_steer = 0.0
self.steer_filter.x = 0.0
output_steer = 0
else:
# Expected actuator value
self.steer_filter.update_alpha(self.RC)
self.steer_filter.update(self.output_steer)
self.steer_filter.update(last_actuators.steer)
# Compute acceleration error
rate_sp = self.outer_loop_gain * (steers_des - self.x[0]) + rate_des
@ -119,21 +102,13 @@ class LatControlINDI():
delta_u = g_inv * accel_error
# If steering pressed, only allow wind down
if CS.steeringPressed and (delta_u * self.output_steer > 0):
if CS.steeringPressed and (delta_u * last_actuators.steer > 0):
delta_u = 0
# Enforce rate limit
if self.enforce_rate_limit:
steer_max = float(CarControllerParams.STEER_MAX)
new_output_steer_cmd = steer_max * (self.steer_filter.x + delta_u)
prev_output_steer_cmd = steer_max * self.output_steer
new_output_steer_cmd = apply_toyota_steer_torque_limits(new_output_steer_cmd, prev_output_steer_cmd, prev_output_steer_cmd, CarControllerParams)
self.output_steer = new_output_steer_cmd / steer_max
else:
self.output_steer = self.steer_filter.x + delta_u
output_steer = self.steer_filter.x + delta_u
steers_max = get_steer_max(CP, CS.vEgo)
self.output_steer = clip(self.output_steer, -steers_max, steers_max)
output_steer = clip(output_steer, -steers_max, steers_max)
indi_log.active = True
indi_log.rateSetPoint = float(rate_sp)
@ -141,9 +116,7 @@ class LatControlINDI():
indi_log.accelError = float(accel_error)
indi_log.delayedOutput = float(self.steer_filter.x)
indi_log.delta = float(delta_u)
indi_log.output = float(self.output_steer)
check_saturation = (CS.vEgo > 10.) and not CS.steeringRateLimited and not CS.steeringPressed
indi_log.saturated = self._check_saturation(self.output_steer, check_saturation, steers_max)
indi_log.output = float(output_steer)
indi_log.saturated = self._check_saturation(steers_max - abs(output_steer) < 1e-3, CS)
return float(self.output_steer), float(steers_des), indi_log
return float(output_steer), float(steers_des), indi_log

30
selfdrive/controls/lib/latcontrol_lqr.py
Unescape View File

@ -5,10 +5,12 @@ from common.numpy_fast import clip
from common.realtime import DT_CTRL
from cereal import log
from selfdrive.controls.lib.drive_helpers import get_steer_max
from selfdrive.controls.lib.latcontrol import LatControl, MIN_STEER_SPEED
class LatControlLQR():
def __init__(self, CP):
class LatControlLQR(LatControl):
def __init__(self, CP, CI):
super().__init__(CP, CI)
self.scale = CP.lateralTuning.lqr.scale
self.ki = CP.lateralTuning.lqr.ki
@ -23,26 +25,11 @@ class LatControlLQR():
self.i_unwind_rate = 0.3 * DT_CTRL
self.i_rate = 1.0 * DT_CTRL
self.sat_count_rate = 1.0 * DT_CTRL
self.sat_limit = CP.steerLimitTimer
self.reset()
def reset(self):
super().reset()
self.i_lqr = 0.0
self.sat_count = 0.0
def _check_saturation(self, control, check_saturation, limit):
saturated = abs(control) == limit
if saturated and check_saturation:
self.sat_count += self.sat_count_rate
else:
self.sat_count -= self.sat_count_rate
self.sat_count = clip(self.sat_count, 0.0, 1.0)
return self.sat_count > self.sat_limit
def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate):
lqr_log = log.ControlsState.LateralLQRState.new_message()
@ -64,7 +51,7 @@ class LatControlLQR():
e = steering_angle_no_offset - angle_steers_k
self.x_hat = self.A.dot(self.x_hat) + self.B.dot(CS.steeringTorqueEps / torque_scale) + self.L.dot(e)
if CS.vEgo < 0.3 or not active:
if CS.vEgo < MIN_STEER_SPEED or not active:
lqr_log.active = False
lqr_output = 0.
output_steer = 0.
@ -91,12 +78,9 @@ class LatControlLQR():
output_steer = lqr_output + self.i_lqr
output_steer = clip(output_steer, -steers_max, steers_max)
check_saturation = (CS.vEgo > 10) and not CS.steeringRateLimited and not CS.steeringPressed
saturated = self._check_saturation(output_steer, check_saturation, steers_max)
lqr_log.steeringAngleDeg = angle_steers_k
lqr_log.i = self.i_lqr
lqr_log.output = output_steer
lqr_log.lqrOutput = lqr_output
lqr_log.saturated = saturated
lqr_log.saturated = self._check_saturation(steers_max - abs(output_steer) < 1e-3, CS)
return output_steer, desired_angle, lqr_log

15
selfdrive/controls/lib/latcontrol_pid.py
Unescape View File

@ -2,18 +2,20 @@ import math
from selfdrive.controls.lib.pid import PIController
from selfdrive.controls.lib.drive_helpers import get_steer_max
from selfdrive.controls.lib.latcontrol import LatControl, MIN_STEER_SPEED
from cereal import log
class LatControlPID():
class LatControlPID(LatControl):
def __init__(self, CP, CI):
super().__init__(CP, CI)
self.pid = PIController((CP.lateralTuning.pid.kpBP, CP.lateralTuning.pid.kpV),
(CP.lateralTuning.pid.kiBP, CP.lateralTuning.pid.kiV),
k_f=CP.lateralTuning.pid.kf, pos_limit=1.0, neg_limit=-1.0,
sat_limit=CP.steerLimitTimer)
k_f=CP.lateralTuning.pid.kf, pos_limit=1.0, neg_limit=-1.0)
self.get_steer_feedforward = CI.get_steer_feedforward_function()
def reset(self):
super().reset()
self.pid.reset()
def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate):
@ -26,7 +28,7 @@ class LatControlPID():
pid_log.steeringAngleDesiredDeg = angle_steers_des
pid_log.angleError = angle_steers_des - CS.steeringAngleDeg
if CS.vEgo < 0.3 or not active:
if CS.vEgo < MIN_STEER_SPEED or not active:
output_steer = 0.0
pid_log.active = False
self.pid.reset()
@ -40,14 +42,13 @@ class LatControlPID():
deadzone = 0.0
check_saturation = (CS.vEgo > 10) and not CS.steeringRateLimited and not CS.steeringPressed
output_steer = self.pid.update(angle_steers_des, CS.steeringAngleDeg, check_saturation=check_saturation, override=CS.steeringPressed,
output_steer = self.pid.update(angle_steers_des, CS.steeringAngleDeg, override=CS.steeringPressed,
feedforward=steer_feedforward, speed=CS.vEgo, deadzone=deadzone)
pid_log.active = True
pid_log.p = self.pid.p
pid_log.i = self.pid.i
pid_log.f = self.pid.f
pid_log.output = output_steer
pid_log.saturated = bool(self.pid.saturated)
pid_log.saturated = self._check_saturation(steers_max - abs(output_steer) < 1e-3, CS)
return output_steer, angle_steers_des, pid_log

39
selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py
Unescape View File

@ -3,6 +3,8 @@ import os
import numpy as np
from casadi import SX, vertcat, sin, cos
from common.realtime import sec_since_boot
from selfdrive.controls.lib.drive_helpers import LAT_MPC_N as N
from selfdrive.controls.lib.drive_helpers import T_IDXS
@ -15,7 +17,8 @@ else:
LAT_MPC_DIR = os.path.dirname(os.path.abspath(__file__))
EXPORT_DIR = os.path.join(LAT_MPC_DIR, "c_generated_code")
JSON_FILE = "acados_ocp_lat.json"
X_DIM = 6
X_DIM = 4
P_DIM = 2
def gen_lat_model():
model = AcadosModel()
@ -26,9 +29,12 @@ def gen_lat_model():
y_ego = SX.sym('y_ego')
psi_ego = SX.sym('psi_ego')
curv_ego = SX.sym('curv_ego')
model.x = vertcat(x_ego, y_ego, psi_ego, curv_ego)
# parameters
v_ego = SX.sym('v_ego')
rotation_radius = SX.sym('rotation_radius')
model.x = vertcat(x_ego, y_ego, psi_ego, curv_ego, v_ego, rotation_radius)
model.p = vertcat(v_ego, rotation_radius)
# controls
curv_rate = SX.sym('curv_rate')
@ -39,18 +45,14 @@ def gen_lat_model():
y_ego_dot = SX.sym('y_ego_dot')
psi_ego_dot = SX.sym('psi_ego_dot')
curv_ego_dot = SX.sym('curv_ego_dot')
v_ego_dot = SX.sym('v_ego_dot')
rotation_radius_dot = SX.sym('rotation_radius_dot')
model.xdot = vertcat(x_ego_dot, y_ego_dot, psi_ego_dot, curv_ego_dot,
v_ego_dot, rotation_radius_dot)
model.xdot = vertcat(x_ego_dot, y_ego_dot, psi_ego_dot, curv_ego_dot)
# dynamics model
f_expl = vertcat(v_ego * cos(psi_ego) - rotation_radius * sin(psi_ego) * (v_ego * curv_ego),
v_ego * sin(psi_ego) + rotation_radius * cos(psi_ego) * (v_ego * curv_ego),
v_ego * curv_ego,
curv_rate,
0.0,
0.0)
curv_rate)
model.f_impl_expr = model.xdot - f_expl
model.f_expl_expr = f_expl
return model
@ -77,8 +79,9 @@ def gen_lat_mpc_solver():
y_ego, psi_ego = ocp.model.x[1], ocp.model.x[2]
curv_rate = ocp.model.u[0]
v_ego = ocp.model.x[4]
v_ego = ocp.model.p[0]
ocp.parameter_values = np.zeros((P_DIM, ))
ocp.cost.yref = np.zeros((3, ))
ocp.cost.yref_e = np.zeros((2, ))
@ -94,7 +97,7 @@ def gen_lat_mpc_solver():
ocp.constraints.idxbx = np.array([2,3])
ocp.constraints.ubx = np.array([np.radians(90), np.radians(50)])
ocp.constraints.lbx = np.array([-np.radians(90), -np.radians(50)])
x0 = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0])
x0 = np.zeros((X_DIM,))
ocp.constraints.x0 = x0
ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM'
@ -102,7 +105,7 @@ def gen_lat_mpc_solver():
ocp.solver_options.integrator_type = 'ERK'
ocp.solver_options.nlp_solver_type = 'SQP_RTI'
ocp.solver_options.qp_solver_iter_max = 1
ocp.solver_options.qp_solver_cond_N = N//4
ocp.solver_options.qp_solver_cond_N = 1
# set prediction horizon
ocp.solver_options.tf = Tf
@ -128,10 +131,12 @@ class LateralMpc():
# Somehow needed for stable init
for i in range(N+1):
self.solver.set(i, 'x', np.zeros(X_DIM))
self.solver.set(i, 'p', np.zeros(P_DIM))
self.solver.constraints_set(0, "lbx", x0)
self.solver.constraints_set(0, "ubx", x0)
self.solver.solve()
self.solution_status = 0
self.solve_time = 0.0
self.cost = 0
def set_weights(self, path_weight, heading_weight, steer_rate_weight):
@ -141,17 +146,25 @@ class LateralMpc():
#TODO hacky weights to keep behavior the same
self.solver.cost_set(N, 'W', (3/20.)*W[:2,:2])
def run(self, x0, v_ego, car_rotation_radius, y_pts, heading_pts):
def run(self, x0, p, y_pts, heading_pts):
x0_cp = np.copy(x0)
p_cp = np.copy(p)
self.solver.constraints_set(0, "lbx", x0_cp)
self.solver.constraints_set(0, "ubx", x0_cp)
self.yref[:,0] = y_pts
v_ego = p_cp[0]
# rotation_radius = p_cp[1]
self.yref[:,1] = heading_pts*(v_ego+5.0)
for i in range(N):
self.solver.cost_set(i, "yref", self.yref[i])
self.solver.set(i, "p", p_cp)
self.solver.set(N, "p", p_cp)
self.solver.cost_set(N, "yref", self.yref[N][:2])
t = sec_since_boot()
self.solution_status = self.solver.solve()
self.solve_time = sec_since_boot() - t
for i in range(N+1):
self.x_sol[i] = self.solver.get(i, 'x')
for i in range(N):

146
selfdrive/controls/lib/lateral_planner.py
Unescape View File

@ -1,4 +1,3 @@
import math
import numpy as np
from common.realtime import sec_since_boot, DT_MDL
from common.numpy_fast import interp
@ -6,54 +5,20 @@ from selfdrive.swaglog import cloudlog
from selfdrive.controls.lib.lateral_mpc_lib.lat_mpc import LateralMpc
from selfdrive.controls.lib.drive_helpers import CONTROL_N, MPC_COST_LAT, LAT_MPC_N, CAR_ROTATION_RADIUS
from selfdrive.controls.lib.lane_planner import LanePlanner, TRAJECTORY_SIZE
from selfdrive.config import Conversions as CV
from selfdrive.controls.lib.desire_helper import DesireHelper
import cereal.messaging as messaging
from cereal import log
LaneChangeState = log.LateralPlan.LaneChangeState
LaneChangeDirection = log.LateralPlan.LaneChangeDirection
LANE_CHANGE_SPEED_MIN = 30 * CV.MPH_TO_MS
LANE_CHANGE_TIME_MAX = 10.
DESIRES = {
LaneChangeDirection.none: {
LaneChangeState.off: log.LateralPlan.Desire.none,
LaneChangeState.preLaneChange: log.LateralPlan.Desire.none,
LaneChangeState.laneChangeStarting: log.LateralPlan.Desire.none,
LaneChangeState.laneChangeFinishing: log.LateralPlan.Desire.none,
},
LaneChangeDirection.left: {
LaneChangeState.off: log.LateralPlan.Desire.none,
LaneChangeState.preLaneChange: log.LateralPlan.Desire.none,
LaneChangeState.laneChangeStarting: log.LateralPlan.Desire.laneChangeLeft,
LaneChangeState.laneChangeFinishing: log.LateralPlan.Desire.laneChangeLeft,
},
LaneChangeDirection.right: {
LaneChangeState.off: log.LateralPlan.Desire.none,
LaneChangeState.preLaneChange: log.LateralPlan.Desire.none,
LaneChangeState.laneChangeStarting: log.LateralPlan.Desire.laneChangeRight,
LaneChangeState.laneChangeFinishing: log.LateralPlan.Desire.laneChangeRight,
},
}
class LateralPlanner:
def __init__(self, CP, use_lanelines=True, wide_camera=False):
self.use_lanelines = use_lanelines
self.LP = LanePlanner(wide_camera)
self.DH = DesireHelper()
self.last_cloudlog_t = 0
self.steer_rate_cost = CP.steerRateCost
self.solution_invalid_cnt = 0
self.lane_change_state = LaneChangeState.off
self.lane_change_direction = LaneChangeDirection.none
self.lane_change_timer = 0.0
self.lane_change_ll_prob = 1.0
self.keep_pulse_timer = 0.0
self.prev_one_blinker = False
self.desire = log.LateralPlan.Desire.none
self.path_xyz = np.zeros((TRAJECTORY_SIZE, 3))
self.path_xyz_stds = np.ones((TRAJECTORY_SIZE, 3))
@ -62,19 +27,19 @@ class LateralPlanner:
self.y_pts = np.zeros(TRAJECTORY_SIZE)
self.lat_mpc = LateralMpc()
self.reset_mpc(np.zeros(6))
self.reset_mpc(np.zeros(4))
def reset_mpc(self, x0=np.zeros(6)):
def reset_mpc(self, x0=np.zeros(4)):
self.x0 = x0
self.lat_mpc.reset(x0=self.x0)
def update(self, sm):
v_ego = sm['carState'].vEgo
active = sm['controlsState'].active
measured_curvature = sm['controlsState'].curvature
# Parse model predictions
md = sm['modelV2']
self.LP.parse_model(sm['modelV2'])
self.LP.parse_model(md)
if len(md.position.x) == TRAJECTORY_SIZE and len(md.orientation.x) == TRAJECTORY_SIZE:
self.path_xyz = np.column_stack([md.position.x, md.position.y, md.position.z])
self.t_idxs = np.array(md.position.t)
@ -83,84 +48,15 @@ class LateralPlanner:
self.path_xyz_stds = np.column_stack([md.position.xStd, md.position.yStd, md.position.zStd])
# Lane change logic
one_blinker = sm['carState'].leftBlinker != sm['carState'].rightBlinker
below_lane_change_speed = v_ego < LANE_CHANGE_SPEED_MIN
if (not active) or (self.lane_change_timer > LANE_CHANGE_TIME_MAX):
self.lane_change_state = LaneChangeState.off
self.lane_change_direction = LaneChangeDirection.none
else:
# LaneChangeState.off
if self.lane_change_state == LaneChangeState.off and one_blinker and not self.prev_one_blinker and not below_lane_change_speed:
self.lane_change_state = LaneChangeState.preLaneChange
self.lane_change_ll_prob = 1.0
# LaneChangeState.preLaneChange
elif self.lane_change_state == LaneChangeState.preLaneChange:
# Set lane change direction
if sm['carState'].leftBlinker:
self.lane_change_direction = LaneChangeDirection.left
elif sm['carState'].rightBlinker:
self.lane_change_direction = LaneChangeDirection.right
else: # If there are no blinkers we will go back to LaneChangeState.off
self.lane_change_direction = LaneChangeDirection.none
torque_applied = sm['carState'].steeringPressed and \
((sm['carState'].steeringTorque > 0 and self.lane_change_direction == LaneChangeDirection.left) or
(sm['carState'].steeringTorque < 0 and self.lane_change_direction == LaneChangeDirection.right))
blindspot_detected = ((sm['carState'].leftBlindspot and self.lane_change_direction == LaneChangeDirection.left) or
(sm['carState'].rightBlindspot and self.lane_change_direction == LaneChangeDirection.right))
if not one_blinker or below_lane_change_speed:
self.lane_change_state = LaneChangeState.off
elif torque_applied and not blindspot_detected:
self.lane_change_state = LaneChangeState.laneChangeStarting
# LaneChangeState.laneChangeStarting
elif self.lane_change_state == LaneChangeState.laneChangeStarting:
# fade out over .5s
self.lane_change_ll_prob = max(self.lane_change_ll_prob - 2 * DT_MDL, 0.0)
# 98% certainty
lane_change_prob = self.LP.l_lane_change_prob + self.LP.r_lane_change_prob
if lane_change_prob < 0.02 and self.lane_change_ll_prob < 0.01:
self.lane_change_state = LaneChangeState.laneChangeFinishing
# LaneChangeState.laneChangeFinishing
elif self.lane_change_state == LaneChangeState.laneChangeFinishing:
# fade in laneline over 1s
self.lane_change_ll_prob = min(self.lane_change_ll_prob + DT_MDL, 1.0)
if self.lane_change_ll_prob > 0.99:
self.lane_change_direction = LaneChangeDirection.none
if one_blinker:
self.lane_change_state = LaneChangeState.preLaneChange
else:
self.lane_change_state = LaneChangeState.off
if self.lane_change_state in (LaneChangeState.off, LaneChangeState.preLaneChange):
self.lane_change_timer = 0.0
else:
self.lane_change_timer += DT_MDL
self.prev_one_blinker = one_blinker
self.desire = DESIRES[self.lane_change_direction][self.lane_change_state]
# Send keep pulse once per second during LaneChangeStart.preLaneChange
if self.lane_change_state in (LaneChangeState.off, LaneChangeState.laneChangeStarting):
self.keep_pulse_timer = 0.0
elif self.lane_change_state == LaneChangeState.preLaneChange:
self.keep_pulse_timer += DT_MDL
if self.keep_pulse_timer > 1.0:
self.keep_pulse_timer = 0.0
elif self.desire in (log.LateralPlan.Desire.keepLeft, log.LateralPlan.Desire.keepRight):
self.desire = log.LateralPlan.Desire.none
lane_change_prob = self.LP.l_lane_change_prob + self.LP.r_lane_change_prob
self.DH.update(sm['carState'], sm['controlsState'].active, lane_change_prob)
# Turn off lanes during lane change
if self.desire == log.LateralPlan.Desire.laneChangeRight or self.desire == log.LateralPlan.Desire.laneChangeLeft:
self.LP.lll_prob *= self.lane_change_ll_prob
self.LP.rll_prob *= self.lane_change_ll_prob
if self.DH.desire == log.LateralPlan.Desire.laneChangeRight or self.DH.desire == log.LateralPlan.Desire.laneChangeLeft:
self.LP.lll_prob *= self.DH.lane_change_ll_prob
self.LP.rll_prob *= self.DH.lane_change_ll_prob
# Calculate final driving path and set MPC costs
if self.use_lanelines:
d_path_xyz = self.LP.get_d_path(v_ego, self.t_idxs, self.path_xyz)
self.lat_mpc.set_weights(MPC_COST_LAT.PATH, MPC_COST_LAT.HEADING, self.steer_rate_cost)
@ -170,23 +66,24 @@ class LateralPlanner:
# Heading cost is useful at low speed, otherwise end of plan can be off-heading
heading_cost = interp(v_ego, [5.0, 10.0], [MPC_COST_LAT.HEADING, 0.0])
self.lat_mpc.set_weights(path_cost, heading_cost, self.steer_rate_cost)
y_pts = np.interp(v_ego * self.t_idxs[:LAT_MPC_N + 1], np.linalg.norm(d_path_xyz, axis=1), d_path_xyz[:, 1])
heading_pts = np.interp(v_ego * self.t_idxs[:LAT_MPC_N + 1], np.linalg.norm(self.path_xyz, axis=1), self.plan_yaw)
self.y_pts = y_pts
assert len(y_pts) == LAT_MPC_N + 1
assert len(heading_pts) == LAT_MPC_N + 1
self.x0[4] = v_ego
# self.x0[4] = v_ego
p = np.array([v_ego, CAR_ROTATION_RADIUS])
self.lat_mpc.run(self.x0,
v_ego,
CAR_ROTATION_RADIUS,
p,
y_pts,
heading_pts)
# init state for next
self.x0[3] = interp(DT_MDL, self.t_idxs[:LAT_MPC_N + 1], self.lat_mpc.x_sol[:, 3])
# Check for infeasible MPC solution
mpc_nans = any(math.isnan(x) for x in self.lat_mpc.x_sol[:, 3])
mpc_nans = np.isnan(self.lat_mpc.x_sol[:, 3]).any()
t = sec_since_boot()
if mpc_nans or self.lat_mpc.solution_status != 0:
self.reset_mpc()
@ -216,10 +113,11 @@ class LateralPlanner:
lateralPlan.dProb = float(self.LP.d_prob)
lateralPlan.mpcSolutionValid = bool(plan_solution_valid)
lateralPlan.solverExecutionTime = self.lat_mpc.solve_time
lateralPlan.desire = self.desire
lateralPlan.desire = self.DH.desire
lateralPlan.useLaneLines = self.use_lanelines
lateralPlan.laneChangeState = self.lane_change_state
lateralPlan.laneChangeDirection = self.lane_change_direction
lateralPlan.laneChangeState = self.DH.lane_change_state
lateralPlan.laneChangeDirection = self.DH.lane_change_direction
pm.send('lateralPlan', plan_send)

Some files were not shown because too many files have changed in this diff Show More

Write Preview
Loading…
Cancel
Save