Merge branch 'master' into docs-c

pull/23262/head
Willem Melching 3 years ago
commit 22da088692
  1. 7
      .github/workflows/selfdrive_tests.yaml
  2. 15
      .github/workflows/tools_tests.yaml
  3. 1
      .gitignore
  4. 8
      .pre-commit-config.yaml
  5. 74
      Dockerfile.openpilot_base
  6. 3
      Pipfile
  7. 1155
      Pipfile.lock
  8. 10
      RELEASES.md
  9. 1
      SConstruct
  10. 2
      cereal
  11. 2
      common/ffi_wrapper.py
  12. 2
      common/file_helpers.py
  13. 2
      common/profiler.py
  14. 4
      common/realtime.py
  15. 2
      common/spinner.py
  16. 2
      common/tests/test_file_helpers.py
  17. 2
      common/timeout.py
  18. 7
      docs/CARS.md
  19. 52
      docs/CONTRIBUTING.md
  20. BIN
      docs/_static/favicon.ico
  21. BIN
      docs/_static/logo.png
  22. 2
      docs/_static/robots.txt
  23. 91
      docs/conf.py
  24. 2
      opendbc
  25. 2
      panda
  26. 39
      selfdrive/athena/athenad.py
  27. 6
      selfdrive/athena/manage_athenad.py
  28. 22
      selfdrive/athena/tests/helpers.py
  29. 27
      selfdrive/athena/tests/test_athenad.py
  30. 46
      selfdrive/boardd/boardd.cc
  31. 4
      selfdrive/boardd/panda.cc
  32. 2
      selfdrive/boardd/panda.h
  33. 2
      selfdrive/boardd/tests/test_boardd_loopback.py
  34. 11
      selfdrive/camerad/cameras/camera_common.cc
  35. 3
      selfdrive/camerad/cameras/camera_common.h
  36. 45
      selfdrive/camerad/cameras/camera_qcom.cc
  37. 1
      selfdrive/camerad/cameras/camera_qcom.h
  38. 7
      selfdrive/camerad/cameras/camera_qcom2.cc
  39. 4
      selfdrive/camerad/cameras/camera_replay.cc
  40. 4
      selfdrive/camerad/cameras/camera_webcam.cc
  41. 8
      selfdrive/camerad/imgproc/utils.cc
  42. 8
      selfdrive/camerad/imgproc/utils.h
  43. 4
      selfdrive/camerad/main.cc
  44. 2
      selfdrive/camerad/test/test_camerad.py
  45. 10
      selfdrive/car/car_helpers.py
  46. 9
      selfdrive/car/chrysler/carcontroller.py
  47. 6
      selfdrive/car/chrysler/interface.py
  48. 4
      selfdrive/car/fingerprints.py
  49. 4
      selfdrive/car/ford/carcontroller.py
  50. 4
      selfdrive/car/ford/interface.py
  51. 19
      selfdrive/car/fw_versions.py
  52. 30
      selfdrive/car/gm/carcontroller.py
  53. 5
      selfdrive/car/gm/carstate.py
  54. 13
      selfdrive/car/gm/interface.py
  55. 1
      selfdrive/car/gm/radar_interface.py
  56. 38
      selfdrive/car/honda/carcontroller.py
  57. 26
      selfdrive/car/honda/interface.py
  58. 12
      selfdrive/car/honda/values.py
  59. 10
      selfdrive/car/hyundai/carcontroller.py
  60. 2
      selfdrive/car/hyundai/hyundaican.py
  61. 11
      selfdrive/car/hyundai/interface.py
  62. 57
      selfdrive/car/hyundai/values.py
  63. 27
      selfdrive/car/interfaces.py
  64. 6
      selfdrive/car/mazda/carcontroller.py
  65. 4
      selfdrive/car/mazda/interface.py
  66. 15
      selfdrive/car/mazda/values.py
  67. 3
      selfdrive/car/mock/interface.py
  68. 5
      selfdrive/car/nissan/carcontroller.py
  69. 11
      selfdrive/car/nissan/interface.py
  70. 23
      selfdrive/car/subaru/carcontroller.py
  71. 41
      selfdrive/car/subaru/carstate.py
  72. 19
      selfdrive/car/subaru/interface.py
  73. 8
      selfdrive/car/subaru/subarucan.py
  74. 76
      selfdrive/car/subaru/values.py
  75. 5
      selfdrive/car/tesla/carcontroller.py
  76. 4
      selfdrive/car/tesla/interface.py
  77. 2
      selfdrive/car/tests/test_car_interfaces.py
  78. 22
      selfdrive/car/toyota/carcontroller.py
  79. 45
      selfdrive/car/toyota/interface.py
  80. 4
      selfdrive/car/toyota/toyotacan.py
  81. 8
      selfdrive/car/toyota/tunes.py
  82. 14
      selfdrive/car/toyota/values.py
  83. 5
      selfdrive/car/volkswagen/carcontroller.py
  84. 17
      selfdrive/car/volkswagen/interface.py
  85. 11
      selfdrive/car/volkswagen/values.py
  86. 34
      selfdrive/common/modeldata.h
  87. 1
      selfdrive/common/params.cc
  88. 4
      selfdrive/common/util.cc
  89. 5
      selfdrive/common/util.h
  90. 2
      selfdrive/common/version.h
  91. 81
      selfdrive/controls/controlsd.py
  92. 101
      selfdrive/controls/lib/events.py
  93. 18
      selfdrive/controls/lib/lane_planner.py
  94. 6
      selfdrive/controls/lib/latcontrol_angle.py
  95. 6
      selfdrive/controls/lib/latcontrol_indi.py
  96. 4
      selfdrive/controls/lib/latcontrol_lqr.py
  97. 6
      selfdrive/controls/lib/latcontrol_pid.py
  98. 5
      selfdrive/controls/lib/lateral_mpc_lib/SConscript
  99. 32
      selfdrive/controls/lib/lateral_planner.py
  100. 30
      selfdrive/controls/lib/longcontrol.py
  101. Some files were not shown because too many files have changed in this diff Show More

@ -351,11 +351,12 @@ jobs:
name: longitudinal name: longitudinal
path: selfdrive/test/longitudinal_maneuvers/out/longitudinal/ path: selfdrive/test/longitudinal_maneuvers/out/longitudinal/
test_car_models: test_cars:
name: car models name: cars
runs-on: ubuntu-20.04 runs-on: ubuntu-20.04
timeout-minutes: 50 timeout-minutes: 50
strategy: strategy:
fail-fast: false
matrix: matrix:
job: [0, 1, 2, 3] job: [0, 1, 2, 3]
steps: steps:
@ -385,7 +386,7 @@ jobs:
- name: Test car models - name: Test car models
run: | run: |
${{ env.RUN }} "scons -j$(nproc) --test && \ ${{ env.RUN }} "scons -j$(nproc) --test && \
FILEREADER_CACHE=1 coverage run selfdrive/test/test_models.py && \ FILEREADER_CACHE=1 pytest selfdrive/test/test_models.py && \
chmod -R 777 /tmp/comma_download_cache" chmod -R 777 /tmp/comma_download_cache"
env: env:
NUM_JOBS: 4 NUM_JOBS: 4

@ -45,7 +45,20 @@ jobs:
- uses: actions/checkout@v2 - uses: actions/checkout@v2
with: with:
submodules: true submodules: true
lfs: true
# HACK: cache LFS objects since they count against our quota
# https://github.com/actions/checkout/issues/165#issuecomment-657673315
- name: Create LFS file list
run: git lfs ls-files -l | cut -d' ' -f1 | sort > .lfs-assets-id
- name: Restore LFS cache
uses: actions/cache@v2
id: lfs-cache
with:
path: .git/lfs
key: ${{ runner.os }}-lfs-${{ hashFiles('.lfs-assets-id') }}
- name: Git LFS Pull
run: git lfs pull
- name: Build Docker image - name: Build Docker image
run: | run: |
eval "$BUILD" eval "$BUILD"

1
.gitignore vendored

@ -1,4 +1,5 @@
venv/ venv/
.env
.clang-format .clang-format
.DS_Store .DS_Store
.tags .tags

@ -13,14 +13,14 @@ repos:
rev: v0.910-1 rev: v0.910-1
hooks: hooks:
- id: mypy - id: mypy
exclude: '^(pyextra)|(cereal)|(rednose)|(panda)|(laika)|(opendbc)|(laika_repo)|(rednose_repo)/' exclude: '^(pyextra/)|(cereal/)|(rednose/)|(panda/)|(laika/)|(opendbc/)|(laika_repo/)|(rednose_repo/)/'
additional_dependencies: ['git+https://github.com/numpy/numpy-stubs', 'types-requests', 'types-atomicwrites', additional_dependencies: ['git+https://github.com/numpy/numpy-stubs', 'types-requests', 'types-atomicwrites',
'types-pycurl'] 'types-pycurl']
- repo: https://github.com/PyCQA/flake8 - repo: https://github.com/PyCQA/flake8
rev: 4.0.1 rev: 4.0.1
hooks: hooks:
- id: flake8 - id: flake8
exclude: '^(pyextra)|(cereal)|(rednose)|(panda)|(laika)|(opendbc)|(laika_repo)|(rednose_repo)|(selfdrive/debug)/' exclude: '^(pyextra/)|(cereal/)|(rednose/)|(panda/)|(laika/)|(opendbc/)|(laika_repo/)|(rednose_repo/)|(selfdrive/debug/)/'
args: args:
- --select=F,E112,E113,E304,E502,E701,E702,E703,E71,E72,E731,W191,W6 - --select=F,E112,E113,E304,E502,E701,E702,E703,E71,E72,E731,W191,W6
- --statistics - --statistics
@ -31,7 +31,7 @@ repos:
entry: pylint entry: pylint
language: system language: system
types: [python] types: [python]
exclude: '^(pyextra)|(cereal)|(rednose)|(panda)|(laika)|(laika_repo)|(rednose_repo)/' exclude: '^(pyextra/)|(cereal/)|(rednose/)|(panda/)|(laika/)|(laika_repo/)|(rednose_repo/)/'
- repo: local - repo: local
hooks: hooks:
- id: cppcheck - id: cppcheck
@ -39,7 +39,7 @@ repos:
entry: cppcheck entry: cppcheck
language: system language: system
types: [c++] types: [c++]
exclude: '^(third_party)|(pyextra)|(cereal)|(opendbc)|(panda)|(tools)|(selfdrive/modeld/thneed/debug)|(selfdrive/modeld/test)|(selfdrive/camerad/test)/|(installer)' exclude: '^(third_party/)|(pyextra/)|(cereal/)|(opendbc/)|(panda/)|(tools/)|(selfdrive/modeld/thneed/debug/)|(selfdrive/modeld/test/)|(selfdrive/camerad/test/)/|(installer/)'
args: args:
- --error-exitcode=1 - --error-exitcode=1
- --language=c++ - --language=c++

@ -1,70 +1,26 @@
FROM ubuntu:20.04 FROM ubuntu:20.04
ENV PYTHONUNBUFFERED 1 ENV PYTHONUNBUFFERED 1
ENV DEBIAN_FRONTEND=noninteractive ENV DEBIAN_FRONTEND=noninteractive
RUN apt-get update && apt-get install -y --no-install-recommends \ RUN apt-get update && \
autoconf \ apt-get install -y --no-install-recommends sudo tzdata locales && \
build-essential \ rm -rf /var/lib/apt/lists/*
bzip2 \
ca-certificates \
capnproto \
clang \
cmake \
cppcheck \
curl \
ffmpeg \
gcc-arm-none-eabi \
git \
iputils-ping \
libarchive-dev \
libbz2-dev \
libcapnp-dev \
libcurl4-openssl-dev \
libeigen3-dev \
libffi-dev \
libgles2-mesa-dev \
libglew-dev \
libglib2.0-0 \
liblzma-dev \
libomp-dev \
libopencv-dev \
libqt5sql5-sqlite \
libqt5svg5-dev \
libsqlite3-dev \
libssl-dev \
libsystemd-dev \
libusb-1.0-0-dev \
libzmq3-dev \
locales \
ocl-icd-libopencl1 \
ocl-icd-opencl-dev \
opencl-headers \
python-dev \
qml-module-qtquick2 \
qt5-default \
qtlocation5-dev \
qtmultimedia5-dev \
qtpositioning5-dev \
qtwebengine5-dev \
sudo \
valgrind \
wget \
&& rm -rf /var/lib/apt/lists/*
RUN sed -i -e 's/# en_US.UTF-8 UTF-8/en_US.UTF-8 UTF-8/' /etc/locale.gen && locale-gen RUN sed -i -e 's/# en_US.UTF-8 UTF-8/en_US.UTF-8 UTF-8/' /etc/locale.gen && locale-gen
ENV LANG en_US.UTF-8 ENV LANG en_US.UTF-8
ENV LANGUAGE en_US:en ENV LANGUAGE en_US:en
ENV LC_ALL en_US.UTF-8 ENV LC_ALL en_US.UTF-8
RUN curl -L https://github.com/pyenv/pyenv-installer/raw/master/bin/pyenv-installer | bash ENV PIPENV_SYSTEM=1
ENV PATH="/root/.pyenv/bin:/root/.pyenv/shims:${PATH}" COPY Pipfile Pipfile.lock .python-version update_requirements.sh /tmp/
COPY tools/ubuntu_setup.sh /tmp/tools/
COPY Pipfile Pipfile.lock /tmp/ RUN cd /tmp && \
RUN pyenv install 3.8.10 && \ tools/ubuntu_setup.sh && \
pyenv global 3.8.10 && \ rm -rf /tmp/* && \
pyenv rehash && \ rm -rf /var/lib/apt/lists/* && \
pip install --no-cache-dir --upgrade pip==21.3.1 && \
pip install --no-cache-dir pipenv==2021.5.29 && \
cd /tmp && \
pipenv install --system --deploy --dev --clear && \
pip uninstall -y pipenv pip uninstall -y pipenv
ENV PYENV_VERSION=3.8.10
ENV PYENV_ROOT="/root/.pyenv"
ENV PATH="$PYENV_ROOT/bin:$PYENV_ROOT/shims:$PATH"

@ -25,9 +25,12 @@ pre-commit = "*"
pycurl = "*" pycurl = "*"
pygame = "*" pygame = "*"
pyprof2calltree = "*" pyprof2calltree = "*"
pytest = "*"
pytest-xdist = "*"
reverse_geocoder = "*" reverse_geocoder = "*"
scipy = "*" scipy = "*"
sphinx = "*" sphinx = "*"
sphinx-sitemap = "*"
sphinx-rtd-theme = "*" sphinx-rtd-theme = "*"
breathe = "*" breathe = "*"
subprocess32 = "*" subprocess32 = "*"

1155
Pipfile.lock generated

File diff suppressed because it is too large Load Diff

@ -1,7 +1,17 @@
Version 0.8.13 (2022-XX-XX)
========================
* Improved driver monitoring
* Improved camera focus on the comma two
* Subaru ECU firmware fingerprinting thanks to martinl!
* Hyundai Santa Fe Plug-in Hybrid 2022 support thanks to sunnyhaibin!
* Subaru Impreza 2020 support thanks to martinl!
Version 0.8.12 (2021-12-15) Version 0.8.12 (2021-12-15)
======================== ========================
* New driving model * New driving model
* Improved behavior around exits * Improved behavior around exits
* Better pose accuracy at high speeds, allowing max speed of 90mph
* Fully incorporated comma three data into all parts of training stack
* Improved follow distance * Improved follow distance
* Better longitudinal policy, especially in low speed traffic * Better longitudinal policy, especially in low speed traffic
* New alert sounds * New alert sounds

@ -127,6 +127,7 @@ else:
"/opt/homebrew/lib", "/opt/homebrew/lib",
"/usr/local/opt/openssl/lib", "/usr/local/opt/openssl/lib",
"/opt/homebrew/opt/openssl/lib", "/opt/homebrew/opt/openssl/lib",
f"#third_party/acados/{arch}/lib",
"/System/Library/Frameworks/OpenGL.framework/Libraries", "/System/Library/Frameworks/OpenGL.framework/Libraries",
] ]
cflags += ["-DGL_SILENCE_DEPRECATION"] cflags += ["-DGL_SILENCE_DEPRECATION"]

@ -1 +1 @@
Subproject commit e5a04ab458afd52cf630cc9e35ccdc10efba6688 Subproject commit e83ec3be7d83b8d4ce227e7c2027dd1ca5f930b4

@ -28,7 +28,7 @@ def ffi_wrap(name, c_code, c_header, tmpdir="/tmp/ccache", cflags="", libraries=
try: try:
mod = __import__(cache) mod = __import__(cache)
except Exception: except Exception:
print("cache miss {0}".format(cache)) print(f"cache miss {cache}")
compile_code(cache, c_code, c_header, tmpdir, cflags, libraries) compile_code(cache, c_code, c_header, tmpdir, cflags, libraries)
mod = __import__(cache) mod = __import__(cache)
finally: finally:

@ -35,7 +35,7 @@ def get_tmpdir_on_same_filesystem(path):
if len(parts) > 1 and parts[1] == "scratch": if len(parts) > 1 and parts[1] == "scratch":
return "/scratch/tmp" return "/scratch/tmp"
elif len(parts) > 2 and parts[2] == "runner": elif len(parts) > 2 and parts[2] == "runner":
return "/{}/runner/tmp".format(parts[1]) return f"/{parts[1]}/runner/tmp"
return "/tmp" return "/tmp"

@ -42,4 +42,4 @@ class Profiler():
print("%30s: %9.2f avg: %7.2f percent: %3.0f IGNORED" % (n, ms*1000.0, ms*1000.0/self.iter, ms/self.tot*100)) print("%30s: %9.2f avg: %7.2f percent: %3.0f IGNORED" % (n, ms*1000.0, ms*1000.0/self.iter, ms/self.tot*100))
else: else:
print("%30s: %9.2f avg: %7.2f percent: %3.0f" % (n, ms*1000.0, ms*1000.0/self.iter, ms/self.tot*100)) print("%30s: %9.2f avg: %7.2f percent: %3.0f" % (n, ms*1000.0, ms*1000.0/self.iter, ms/self.tot*100))
print("Iter clock: %2.6f TOTAL: %2.2f" % (self.tot/self.iter, self.tot)) print(f"Iter clock: {self.tot / self.iter:2.6f} TOTAL: {self.tot:2.2f}")

@ -39,7 +39,7 @@ def set_realtime_priority(level: int) -> None:
def set_core_affinity(core: int) -> None: def set_core_affinity(core: int) -> None:
if not PC: if not PC:
os.sched_setaffinity(0, [core,]) os.sched_setaffinity(0, [core,]) # type: ignore[attr-defined]
def config_realtime_process(core: int, priority: int) -> None: def config_realtime_process(core: int, priority: int) -> None:
@ -79,7 +79,7 @@ class Ratekeeper:
remaining = self._next_frame_time - sec_since_boot() remaining = self._next_frame_time - sec_since_boot()
self._next_frame_time += self._interval self._next_frame_time += self._interval
if self._print_delay_threshold is not None and remaining < -self._print_delay_threshold: if self._print_delay_threshold is not None and remaining < -self._print_delay_threshold:
print("%s lagging by %.2f ms" % (self._process_name, -remaining * 1000)) print(f"{self._process_name} lagging by {-remaining * 1000:.2f} ms")
lagged = True lagged = True
self._frame += 1 self._frame += 1
self._remaining = remaining self._remaining = remaining

@ -24,7 +24,7 @@ class Spinner():
except BrokenPipeError: except BrokenPipeError:
pass pass
def update_progress(self, cur: int, total: int): def update_progress(self, cur: float, total: float):
self.update(str(round(100 * cur / total))) self.update(str(round(100 * cur / total)))
def close(self): def close(self):

@ -8,7 +8,7 @@ from common.file_helpers import atomic_write_in_dir
class TestFileHelpers(unittest.TestCase): class TestFileHelpers(unittest.TestCase):
def run_atomic_write_func(self, atomic_write_func): def run_atomic_write_func(self, atomic_write_func):
path = "/tmp/tmp{}".format(uuid4()) path = f"/tmp/tmp{uuid4()}"
with atomic_write_func(path) as f: with atomic_write_func(path) as f:
f.write("test") f.write("test")

@ -12,7 +12,7 @@ class Timeout:
""" """
def __init__(self, seconds, error_msg=None): def __init__(self, seconds, error_msg=None):
if error_msg is None: if error_msg is None:
error_msg = 'Timed out after {} seconds'.format(seconds) error_msg = f'Timed out after {seconds} seconds'
self.seconds = seconds self.seconds = seconds
self.error_msg = error_msg self.error_msg = error_msg

@ -115,6 +115,7 @@
| Hyundai | Santa Fe 2019-20 | All | Stock | 0mph | 0mph | | Hyundai | Santa Fe 2019-20 | All | Stock | 0mph | 0mph |
| Hyundai | Santa Fe 2021-22 | 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 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 2018-2019 | SCC + LKAS | Stock | 0mph | 0mph |
| Hyundai | Sonata Hybrid 2021-22 | All | Stock | 0mph | 0mph | | Hyundai | Sonata Hybrid 2021-22 | All | Stock | 0mph | 0mph |
| Hyundai | Veloster 2019-20 | SCC + LKAS | Stock | 5mph | 0mph | | Hyundai | Veloster 2019-20 | SCC + LKAS | Stock | 5mph | 0mph |
@ -123,7 +124,7 @@
| Kia | Ceed 2019 | SCC + LKAS | Stock | 0mph | 0mph | | Kia | Ceed 2019 | SCC + LKAS | Stock | 0mph | 0mph |
| Kia | Forte 2018-21 | SCC + LKAS | Stock | 0mph | 0mph | | Kia | Forte 2018-21 | SCC + LKAS | Stock | 0mph | 0mph |
| Kia | K5 2021-22 | SCC + LFA | Stock | 0mph | 0mph | | Kia | K5 2021-22 | SCC + LFA | Stock | 0mph | 0mph |
| Kia | Niro EV 2019-21 | SCC + LKAS | Stock | 0mph | 0mph | | Kia | Niro EV 2019-22 | SCC + LKAS | Stock | 0mph | 0mph |
| Kia | Niro Hybrid 2021 | SCC + LKAS | Stock | 0mph | 0mph | | Kia | Niro Hybrid 2021 | SCC + LKAS | Stock | 0mph | 0mph |
| Kia | Niro PHEV 2019 | SCC + LKAS | Stock | 10mph | 32mph | | Kia | Niro PHEV 2019 | SCC + LKAS | Stock | 10mph | 32mph |
| Kia | Optima 2017 | SCC + LKAS | Stock | 0mph | 32mph | | Kia | Optima 2017 | SCC + LKAS | Stock | 0mph | 32mph |
@ -147,10 +148,10 @@
| Škoda | Scala 2020 | Driver Assistance | Stock | 0mph | 0mph | | Škoda | Scala 2020 | Driver Assistance | Stock | 0mph | 0mph |
| Škoda | Superb 2015-18 | Driver Assistance | Stock | 0mph | 0mph | | Škoda | Superb 2015-18 | Driver Assistance | Stock | 0mph | 0mph |
| Subaru | Ascent 2019 | EyeSight | Stock | 0mph | 0mph | | Subaru | Ascent 2019 | EyeSight | Stock | 0mph | 0mph |
| Subaru | Crosstrek 2018-19 | EyeSight | Stock | 0mph | 0mph | | Subaru | Crosstrek 2018-20 | EyeSight | Stock | 0mph | 0mph |
| Subaru | Forester 2019-21 | EyeSight | Stock | 0mph | 0mph | | Subaru | Forester 2019-21 | EyeSight | Stock | 0mph | 0mph |
| Subaru | Impreza 2017-19 | EyeSight | Stock | 0mph | 0mph | | Subaru | Impreza 2017-19 | EyeSight | Stock | 0mph | 0mph |
| Volkswagen| Arteon 2021<sup>4</sup> | Driver Assistance | 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| Atlas 2018-19 | Driver Assistance | Stock | 0mph | 0mph |
| Volkswagen| California 2021<sup>4</sup> | Driver Assistance | Stock | 0mph | 32mph | | Volkswagen| California 2021<sup>4</sup> | Driver Assistance | Stock | 0mph | 32mph |
| Volkswagen| e-Golf 2014, 2019-20 | Driver Assistance | Stock | 0mph | 0mph | | Volkswagen| e-Golf 2014, 2019-20 | Driver Assistance | Stock | 0mph | 0mph |

@ -1,46 +1,48 @@
# How to contribute # How to contribute
Our software is open source so you can solve your own problems without needing help from others. And if you solve a problem and are so kind, you can upstream it for the rest of the world to use. Our software is open source so you can solve your own problems without needing help from others. And if you solve a problem and are so kind, you can upstream it for the rest of the world to use. Check out our [post about externalization](https://blog.comma.ai/a-2020-theme-externalization/).
Most open source development activity is coordinated through our [GitHub Discussions](https://github.com/commaai/openpilot/discussions) and [Discord](https://discord.comma.ai). A lot of documentation is available on our [blog](https://blog.comma.ai/). Most open source development activity is coordinated through our [GitHub Discussions](https://github.com/commaai/openpilot/discussions) and [Discord](https://discord.comma.ai). A lot of documentation is available at https://docs.comma.ai and on our [blog](https://blog.comma.ai/).
## Getting Started ### Getting Started
* Setup your [development environment](../tools/) * Setup your [development environment](../tools/)
* Join our [Discord](https://discord.comma.ai) * Join our [Discord](https://discord.comma.ai)
* Make sure you have a [GitHub account](https://github.com/signup/free) * Make sure you have a [GitHub account](https://github.com/signup/free)
* Fork [our repositories](https://github.com/commaai) on GitHub * Fork [our repositories](https://github.com/commaai) on GitHub
## Testing ### First contribution
Try out some of these first pull requests ideas to dive into the codebase:
### Automated Testing * Increase our [mypy](http://mypy-lang.org/) coverage
* Write some documentation
* Tackle an open [good first issue](https://github.com/commaai/openpilot/issues?q=is%3Aissue+is%3Aopen+label%3A%22good+first+issue%22)
All PRs and commits are automatically checked by GitHub Actions. Check out `.github/workflows/` for what GitHub Actions runs. Any new tests should be added to GitHub Actions. ## Pull Requests
### Code Style and Linting Pull requests should be against the master branch. Welcomed contributions include bug reports, car ports, and any [open issue](https://github.com/commaai/openpilot/issues). If you're unsure about a contribution, feel free to open a discussion, issue, or draft PR to discuss the problem you're trying to solve.
Code is automatically checked for style by GitHub Actions as part of the automated tests. You can also run these tests yourself by running `pre-commit run --all`. A good pull request has all of the following:
* a clearly stated purpose
* every line changed directly contributes to the stated purpose
* verification, i.e. how did you test your PR?
* justification
* if you've optimized something, post benchmarks to prove it's better
* if you've improved your car's tuning, post before and after plots
* passes the CI tests
## Car Ports ### Car Ports
We've released a [Model Port guide](https://blog.comma.ai/openpilot-port-guide-for-toyota-models/) for porting to Toyota/Lexus models. We've released a [Model Port guide](https://blog.comma.ai/openpilot-port-guide-for-toyota-models/) for porting to Toyota/Lexus models.
If you port openpilot to a substantially new car brand, see this more generic [Brand Port guide](https://blog.comma.ai/how-to-write-a-car-port-for-openpilot/). If you port openpilot to a substantially new car brand, see this more generic [Brand Port guide](https://blog.comma.ai/how-to-write-a-car-port-for-openpilot/).
## Pull Requests ## Testing
Pull requests should be against the master branch. Before running master on in-car hardware, you'll need to clone the submodules too. That can be done by recursively cloning the repository: ### Automated Testing
```
git clone https://github.com/commaai/openpilot.git --recursive All PRs and commits are automatically checked by GitHub Actions. Check out `.github/workflows/` for what GitHub Actions runs. Any new tests should be added to GitHub Actions.
```
Or alternatively, when on the master branch: ### Code Style and Linting
```
git submodule update --init Code is automatically checked for style by GitHub Actions as part of the automated tests. You can also run these tests yourself by running `pre-commit run --all`.
```
The reasons for having submodules on a dedicated repository and our new development philosophy can be found in our [post about externalization](https://blog.comma.ai/a-2020-theme-externalization/).
Modules that are in seperate repositories include:
* cereal
* laika
* opendbc
* panda
* rednose

Binary file not shown.

After

Width:  |  Height:  |  Size: 15 KiB

Binary file not shown.

After

Width:  |  Height:  |  Size: 1.1 KiB

@ -0,0 +1,2 @@
User-agent: *
Sitemap: https://docs.comma.ai/sitemap.xml

@ -16,16 +16,22 @@
import os import os
from os.path import exists from os.path import exists
import sys import sys
from selfdrive.version import get_version
from common.basedir import BASEDIR
sys.path.insert(0, os.path.abspath('.')) sys.path.insert(0, os.path.abspath('.'))
sys.path.insert(0, os.path.abspath('..')) sys.path.insert(0, os.path.abspath('..'))
OPENPILOT_ROOT = os.path.abspath(r'../../') # from openpilot/build/docs
VERSION = get_version()
# -- Project information ----------------------------------------------------- # -- Project information -----------------------------------------------------
project = 'openpilot' project = 'openpilot docs'
copyright = '2021, comma.ai' copyright = '2021, comma.ai'
author = 'comma.ai' author = 'comma.ai'
version = VERSION
release = VERSION
language = 'en'
# -- General configuration --------------------------------------------------- # -- General configuration ---------------------------------------------------
@ -39,8 +45,34 @@ extensions = [
'sphinx_rtd_theme', # Read The Docs theme 'sphinx_rtd_theme', # Read The Docs theme
'myst_parser', # Markdown parsing 'myst_parser', # Markdown parsing
'breathe', # Doxygen C/C++ integration 'breathe', # Doxygen C/C++ integration
'sphinx_sitemap', # sitemap generation for SEO
] ]
myst_html_meta = {
"description": "openpilot docs",
"keywords": "op, openpilot, docs, documentation",
"robots": "all,follow",
"googlebot": "index,follow,snippet,archive",
"property=og:locale": "en_US",
"property=og:site_name": "docs.comma.ai",
"property=og:url": "https://docs.comma.ai",
"property=og:title": "openpilot Docuemntation",
"property=og:type": "website",
"property=og:image:type": "image/jpeg",
"property=og:image:width": "400",
"property=og:image": "https://docs.comma.ai/_static/logo.png",
"property=og:image:url": "https://docs.comma.ai/_static/logo.png",
"property=og:image:secure_url": "https://docs.comma.ai/_static/logo.png",
"property=og:description": "openpilot Documentation",
"property=twitter:card": "summary_large_image",
"property=twitter:logo": "https://docs.comma.ai/_static/logo.png",
"property=twitter:title": "openpilot Documentation",
"property=twitter:description": "openpilot Documentation"
}
html_baseurl = 'https://docs.comma.ai/'
sitemap_filename = "sitemap.xml"
# Add any paths that contain templates here, relative to this directory. # Add any paths that contain templates here, relative to this directory.
templates_path = ['_templates'] templates_path = ['_templates']
@ -54,43 +86,38 @@ exclude_patterns = []
# Breathe Configuration # Breathe Configuration
# breathe_default_project = "c_docs" # breathe_default_project = "c_docs"
breathe_build_directory = f"{OPENPILOT_ROOT}/build/docs/html/xml" breathe_build_directory = f"{BASEDIR}/build/docs/html/xml"
breathe_separate_member_pages = True breathe_separate_member_pages = True
breathe_default_members = ('members', 'private-members', 'undoc-members') breathe_default_members = ('members', 'private-members', 'undoc-members')
breathe_domain_by_extension = { breathe_domain_by_extension = {
"h" : "cc", "h": "cc",
} }
breathe_implementation_filename_extensions = ['.c', '.cc', '.cpp'] breathe_implementation_filename_extensions = ['.c', '.cc', '.cpp']
breathe_doxygen_config_options = {} breathe_doxygen_config_options = {}
breathe_projects_source = { breathe_projects_source = {}
# "loggerd" : ("../../../selfdrive/loggerd", ["logger.h"])
}
# only document files that have accompanying .cc files next to them # only document files that have accompanying .cc files next to them
print("searching for c_docs...") print("searching for c_docs...")
for root, dirs, files in os.walk(OPENPILOT_ROOT): for root, dirs, files in os.walk(BASEDIR):
found = False found = False
breath_src = {} breath_src = {}
breathe_srcs_list = [] breathe_srcs_list = []
for file in files:
ccFile = os.path.join(root, file)[:-2] +".cc"
if file.endswith(".h") and exists(ccFile): for file in files:
f = os.path.join(root, file) ccFile = os.path.join(root, file)[:-2] + ".cc"
parent_dir = os.path.basename(os.path.dirname(f))
parent_dir_abs = os.path.dirname(f)
print(f"\tFOUND: {f} in {parent_dir} ({parent_dir_abs})")
breathe_srcs_list.append(file) if file.endswith(".h") and exists(ccFile):
# breathe_srcs_list.append(ccFile) f = os.path.join(root, file)
found = True parent_dir = os.path.basename(os.path.dirname(f))
parent_dir_abs = os.path.dirname(f)
print(f"\tFOUND: {f} in {parent_dir} ({parent_dir_abs})")
# print(f"\tbreathe_srcs_list: {breathe_srcs_list}") breathe_srcs_list.append(file)
found = True
if found: if found:
breath_src[parent_dir] = (parent_dir_abs, breathe_srcs_list) breath_src[parent_dir] = (parent_dir_abs, breathe_srcs_list)
breathe_projects_source.update(breath_src) breathe_projects_source.update(breath_src)
print(f"breathe_projects_source: {breathe_projects_source.keys()}") print(f"breathe_projects_source: {breathe_projects_source.keys()}")
# input("Press Enter to continue...") # input("Press Enter to continue...")
@ -101,8 +128,18 @@ print(f"breathe_projects_source: {breathe_projects_source.keys()}")
# a list of builtin themes. # a list of builtin themes.
# #
html_theme = 'sphinx_rtd_theme' html_theme = 'sphinx_rtd_theme'
html_show_copyright = True
# Add any paths that contain custom static files (such as style sheets) here, # Add any paths that contain custom static files (such as style sheets) here,
# relative to this directory. They are copied after the builtin static files, # relative to this directory. They are copied after the builtin static files,
# so a file named "default.css" will overwrite the builtin "default.css". # so a file named "default.css" will overwrite the builtin "default.css".
html_static_path = ['_static'] html_static_path = ['_static']
html_logo = '_static/logo.png'
html_favicon = '_static/favicon.ico'
html_theme_options = {
'logo_only': False,
'display_version': True,
'vcs_pageview_mode': 'blob',
'style_nav_header_background': '#000000',
}
html_extra_path = ['_static']

@ -1 +1 @@
Subproject commit 61534fd13162d714432c5685f2c59fafe7a96823 Subproject commit 2bab99fd861786312bde676823e21d80eeeb01fa

@ -1 +1 @@
Subproject commit 652367d2e82f21f996c2217857d20ea05567ad62 Subproject commit 79f5e6fe860d4f84876f08ddeea00eb6361cb05d

@ -34,7 +34,7 @@ from selfdrive.version import get_version, get_origin, get_short_branch, get_com
ATHENA_HOST = os.getenv('ATHENA_HOST', 'wss://athena.comma.ai') ATHENA_HOST = os.getenv('ATHENA_HOST', 'wss://athena.comma.ai')
HANDLER_THREADS = int(os.getenv('HANDLER_THREADS', "4")) HANDLER_THREADS = int(os.getenv('HANDLER_THREADS', "4"))
LOCAL_PORT_WHITELIST = set([8022]) LOCAL_PORT_WHITELIST = {8022}
LOG_ATTR_NAME = 'user.upload' LOG_ATTR_NAME = 'user.upload'
LOG_ATTR_VALUE_MAX_UNIX_TIME = int.to_bytes(2147483647, 4, sys.byteorder) LOG_ATTR_VALUE_MAX_UNIX_TIME = int.to_bytes(2147483647, 4, sys.byteorder)
@ -56,6 +56,28 @@ UploadItem = namedtuple('UploadItem', ['path', 'url', 'headers', 'created_at', '
cur_upload_items = {} cur_upload_items = {}
class UploadQueueCache():
params = Params()
@staticmethod
def initialize(upload_queue):
try:
upload_queue_json = UploadQueueCache.params.get("AthenadUploadQueue")
if upload_queue_json is not None:
for item in json.loads(upload_queue_json):
upload_queue.put(UploadItem(**item))
except Exception:
cloudlog.exception("athena.UploadQueueCache.initialize.exception")
@staticmethod
def cache(upload_queue):
try:
items = [i._asdict() for i in upload_queue.queue if i.id not in cancelled_uploads]
UploadQueueCache.params.put("AthenadUploadQueue", json.dumps(items))
except Exception:
cloudlog.exception("athena.UploadQueueCache.cache.exception")
def handle_long_poll(ws): def handle_long_poll(ws):
end_event = threading.Event() end_event = threading.Event()
@ -111,6 +133,7 @@ def upload_handler(end_event):
try: try:
cur_upload_items[tid] = upload_queue.get(timeout=1)._replace(current=True) cur_upload_items[tid] = upload_queue.get(timeout=1)._replace(current=True)
if cur_upload_items[tid].id in cancelled_uploads: if cur_upload_items[tid].id in cancelled_uploads:
cancelled_uploads.remove(cur_upload_items[tid].id) cancelled_uploads.remove(cur_upload_items[tid].id)
continue continue
@ -120,6 +143,7 @@ def upload_handler(end_event):
cur_upload_items[tid] = cur_upload_items[tid]._replace(progress=cur / sz if sz else 1) cur_upload_items[tid] = cur_upload_items[tid]._replace(progress=cur / sz if sz else 1)
_do_upload(cur_upload_items[tid], cb) _do_upload(cur_upload_items[tid], cb)
UploadQueueCache.cache(upload_queue)
except (requests.exceptions.Timeout, requests.exceptions.ConnectionError, requests.exceptions.SSLError) as e: except (requests.exceptions.Timeout, requests.exceptions.ConnectionError, requests.exceptions.SSLError) as e:
cloudlog.warning(f"athena.upload_handler.retry {e} {cur_upload_items[tid]}") cloudlog.warning(f"athena.upload_handler.retry {e} {cur_upload_items[tid]}")
@ -131,6 +155,8 @@ def upload_handler(end_event):
current=False current=False
) )
upload_queue.put_nowait(item) upload_queue.put_nowait(item)
UploadQueueCache.cache(upload_queue)
cur_upload_items[tid] = None cur_upload_items[tid] = None
for _ in range(RETRY_DELAY): for _ in range(RETRY_DELAY):
@ -248,6 +274,7 @@ def uploadFileToUrl(fn, url, headers):
item = item._replace(id=upload_id) item = item._replace(id=upload_id)
upload_queue.put_nowait(item) upload_queue.put_nowait(item)
UploadQueueCache.cache(upload_queue)
return {"enqueued": 1, "item": item._asdict()} return {"enqueued": 1, "item": item._asdict()}
@ -260,7 +287,7 @@ def listUploadQueue():
@dispatcher.add_method @dispatcher.add_method
def cancelUpload(upload_id): def cancelUpload(upload_id):
upload_ids = set(item.id for item in list(upload_queue.queue)) upload_ids = {item.id for item in list(upload_queue.queue)}
if upload_id not in upload_ids: if upload_id not in upload_ids:
return 404 return 404
@ -280,8 +307,7 @@ def startLocalProxy(global_end_event, remote_ws_uri, local_port):
cloudlog.debug("athena.startLocalProxy.starting") cloudlog.debug("athena.startLocalProxy.starting")
params = Params() dongle_id = Params().get("DongleId").decode('utf8')
dongle_id = params.get("DongleId").decode('utf8')
identity_token = Api(dongle_id).get_token() identity_token = Api(dongle_id).get_token()
ws = create_connection(remote_ws_uri, ws = create_connection(remote_ws_uri,
cookie="jwt=" + identity_token, cookie="jwt=" + identity_token,
@ -312,7 +338,7 @@ def getPublicKey():
if not os.path.isfile(PERSIST + '/comma/id_rsa.pub'): if not os.path.isfile(PERSIST + '/comma/id_rsa.pub'):
return None return None
with open(PERSIST + '/comma/id_rsa.pub', 'r') as f: with open(PERSIST + '/comma/id_rsa.pub') as f:
return f.read() return f.read()
@ -393,7 +419,7 @@ def log_handler(end_event):
curr_time = int(time.time()) curr_time = int(time.time())
log_path = os.path.join(SWAGLOG_DIR, log_entry) log_path = os.path.join(SWAGLOG_DIR, log_entry)
setxattr(log_path, LOG_ATTR_NAME, int.to_bytes(curr_time, 4, sys.byteorder)) setxattr(log_path, LOG_ATTR_NAME, int.to_bytes(curr_time, 4, sys.byteorder))
with open(log_path, "r") as f: with open(log_path) as f:
jsonrpc = { jsonrpc = {
"method": "forwardLogs", "method": "forwardLogs",
"params": { "params": {
@ -525,6 +551,7 @@ def backoff(retries):
def main(): def main():
params = Params() params = Params()
dongle_id = params.get("DongleId", encoding='utf-8') dongle_id = params.get("DongleId", encoding='utf-8')
UploadQueueCache.initialize(upload_queue)
ws_uri = ATHENA_HOST + "/ws/v2/" + dongle_id ws_uri = ATHENA_HOST + "/ws/v2/" + dongle_id
api = Api(dongle_id) api = Api(dongle_id)

@ -6,7 +6,7 @@ from multiprocessing import Process
from common.params import Params from common.params import Params
from selfdrive.manager.process import launcher from selfdrive.manager.process import launcher
from selfdrive.swaglog import cloudlog from selfdrive.swaglog import cloudlog
from selfdrive.version import get_version, get_dirty from selfdrive.version import get_version, is_dirty
ATHENA_MGR_PID_PARAM = "AthenadPid" ATHENA_MGR_PID_PARAM = "AthenadPid"
@ -14,12 +14,12 @@ ATHENA_MGR_PID_PARAM = "AthenadPid"
def main(): def main():
params = Params() params = Params()
dongle_id = params.get("DongleId").decode('utf-8') dongle_id = params.get("DongleId").decode('utf-8')
cloudlog.bind_global(dongle_id=dongle_id, version=get_version(), dirty=get_dirty()) cloudlog.bind_global(dongle_id=dongle_id, version=get_version(), dirty=is_dirty())
try: try:
while 1: while 1:
cloudlog.info("starting athena daemon") cloudlog.info("starting athena daemon")
proc = Process(name='athenad', target=launcher, args=('selfdrive.athena.athenad',)) proc = Process(name='athenad', target=launcher, args=('selfdrive.athena.athenad', 'athenad'))
proc.start() proc.start()
proc.join() proc.join()
cloudlog.event("athenad exited", exitcode=proc.exitcode) cloudlog.event("athenad exited", exitcode=proc.exitcode)

@ -50,18 +50,28 @@ class MockApi():
class MockParams(): class MockParams():
def __init__(self): default_params = {
self.params = { "DongleId": b"0000000000000000",
"DongleId": b"0000000000000000", "GithubSshKeys": b"ssh-rsa AAAAB3NzaC1yc2EAAAADAQABAAABAQC307aE+nuHzTAgaJhzSf5v7ZZQW9gaperjhCmyPyl4PzY7T1mDGenTlVTN7yoVFZ9UfO9oMQqo0n1OwDIiqbIFxqnhrHU0cYfj88rI85m5BEKlNu5RdaVTj1tcbaPpQc5kZEolaI1nDDjzV0lwS7jo5VYDHseiJHlik3HH1SgtdtsuamGR2T80q1SyW+5rHoMOJG73IH2553NnWuikKiuikGHUYBd00K1ilVAK2xSiMWJp55tQfZ0ecr9QjEsJ+J/efL4HqGNXhffxvypCXvbUYAFSddOwXUPo5BTKevpxMtH+2YrkpSjocWA04VnTYFiPG6U4ItKmbLOTFZtPzoez private", # noqa: E501
"GithubSshKeys": b"ssh-rsa AAAAB3NzaC1yc2EAAAADAQABAAABAQC307aE+nuHzTAgaJhzSf5v7ZZQW9gaperjhCmyPyl4PzY7T1mDGenTlVTN7yoVFZ9UfO9oMQqo0n1OwDIiqbIFxqnhrHU0cYfj88rI85m5BEKlNu5RdaVTj1tcbaPpQc5kZEolaI1nDDjzV0lwS7jo5VYDHseiJHlik3HH1SgtdtsuamGR2T80q1SyW+5rHoMOJG73IH2553NnWuikKiuikGHUYBd00K1ilVAK2xSiMWJp55tQfZ0ecr9QjEsJ+J/efL4HqGNXhffxvypCXvbUYAFSddOwXUPo5BTKevpxMtH+2YrkpSjocWA04VnTYFiPG6U4ItKmbLOTFZtPzoez private" # noqa: E501 "AthenadUploadQueue": '[]'
} }
params = default_params.copy()
@staticmethod
def restore_defaults():
MockParams.params = MockParams.default_params.copy()
def get(self, k, encoding=None): def get(self, k, encoding=None):
ret = self.params.get(k) ret = MockParams.params.get(k)
if ret is not None and encoding is not None: if ret is not None and encoding is not None:
ret = ret.decode(encoding) ret = ret.decode(encoding)
return ret return ret
def put(self, k, v):
if k not in MockParams.params:
raise KeyError(f"key: {k} not in MockParams")
MockParams.params[k] = v
class MockWebsocket(): class MockWebsocket():
def __init__(self, recv_queue, send_queue): def __init__(self, recv_queue, send_queue):

@ -21,19 +21,22 @@ from selfdrive.athena.athenad import MAX_RETRY_COUNT, dispatcher
from selfdrive.athena.tests.helpers import MockWebsocket, MockParams, MockApi, EchoSocket, with_http_server from selfdrive.athena.tests.helpers import MockWebsocket, MockParams, MockApi, EchoSocket, with_http_server
from cereal import messaging from cereal import messaging
class TestAthenadMethods(unittest.TestCase): class TestAthenadMethods(unittest.TestCase):
@classmethod @classmethod
def setUpClass(cls): def setUpClass(cls):
cls.SOCKET_PORT = 45454 cls.SOCKET_PORT = 45454
athenad.Params = MockParams
athenad.ROOT = tempfile.mkdtemp() athenad.ROOT = tempfile.mkdtemp()
athenad.SWAGLOG_DIR = swaglog.SWAGLOG_DIR = tempfile.mkdtemp() athenad.SWAGLOG_DIR = swaglog.SWAGLOG_DIR = tempfile.mkdtemp()
athenad.Params = MockParams
athenad.Api = MockApi athenad.Api = MockApi
athenad.LOCAL_PORT_WHITELIST = set([cls.SOCKET_PORT]) athenad.LOCAL_PORT_WHITELIST = {cls.SOCKET_PORT}
def setUp(self): def setUp(self):
MockParams.restore_defaults()
athenad.upload_queue = queue.Queue() athenad.upload_queue = queue.Queue()
athenad.cur_upload_items.clear() athenad.cur_upload_items.clear()
athenad.cancelled_uploads.clear()
for i in os.listdir(athenad.ROOT): for i in os.listdir(athenad.ROOT):
p = os.path.join(athenad.ROOT, i) p = os.path.join(athenad.ROOT, i)
@ -249,6 +252,26 @@ class TestAthenadMethods(unittest.TestCase):
items = dispatcher["listUploadQueue"]() items = dispatcher["listUploadQueue"]()
self.assertEqual(len(items), 0) self.assertEqual(len(items), 0)
def test_upload_queue_persistence(self):
item1 = athenad.UploadItem(path="_", url="_", headers={}, created_at=int(time.time()), id='id1')
item2 = athenad.UploadItem(path="_", url="_", headers={}, created_at=int(time.time()), id='id2')
athenad.upload_queue.put_nowait(item1)
athenad.upload_queue.put_nowait(item2)
# Ensure cancelled items are not persisted
athenad.cancelled_uploads.add(item2.id)
# serialize item
athenad.UploadQueueCache.cache(athenad.upload_queue)
# deserialize item
athenad.upload_queue.queue.clear()
athenad.UploadQueueCache.initialize(athenad.upload_queue)
self.assertEqual(athenad.upload_queue.qsize(), 1)
self.assertDictEqual(athenad.upload_queue.queue[-1]._asdict(), item1._asdict())
@mock.patch('selfdrive.athena.athenad.create_connection') @mock.patch('selfdrive.athena.athenad.create_connection')
def test_startLocalProxy(self, mock_create_connection): def test_startLocalProxy(self, mock_create_connection):
end_event = threading.Event() end_event = threading.Event()

@ -205,17 +205,17 @@ Panda *usb_connect(std::string serial="", uint32_t index=0) {
} }
void can_send_thread(std::vector<Panda *> pandas, bool fake_send) { void can_send_thread(std::vector<Panda *> pandas, bool fake_send) {
set_thread_name("boardd_can_send"); util::set_thread_name("boardd_can_send");
AlignedBuffer aligned_buf; AlignedBuffer aligned_buf;
Context * context = Context::create(); std::unique_ptr<Context> context(Context::create());
SubSocket * subscriber = SubSocket::create(context, "sendcan"); std::unique_ptr<SubSocket> subscriber(SubSocket::create(context.get(), "sendcan"));
assert(subscriber != NULL); assert(subscriber != NULL);
subscriber->setTimeout(100); subscriber->setTimeout(100);
// run as fast as messages come in // run as fast as messages come in
while (!do_exit && check_all_connected(pandas)) { while (!do_exit && check_all_connected(pandas)) {
Message * msg = subscriber->receive(); std::unique_ptr<Message> msg(subscriber->receive());
if (!msg) { if (!msg) {
if (errno == EINTR) { if (errno == EINTR) {
do_exit = true; do_exit = true;
@ -223,27 +223,20 @@ void can_send_thread(std::vector<Panda *> pandas, bool fake_send) {
continue; continue;
} }
capnp::FlatArrayMessageReader cmsg(aligned_buf.align(msg)); capnp::FlatArrayMessageReader cmsg(aligned_buf.align(msg.get()));
cereal::Event::Reader event = cmsg.getRoot<cereal::Event>(); cereal::Event::Reader event = cmsg.getRoot<cereal::Event>();
//Dont send if older than 1 second //Dont send if older than 1 second
if (nanos_since_boot() - event.getLogMonoTime() < 1e9) { if ((nanos_since_boot() - event.getLogMonoTime() < 1e9) && !fake_send) {
if (!fake_send) { for (const auto& panda : pandas) {
for (const auto& panda : pandas) { panda->can_send(event.getSendcan());
panda->can_send(event.getSendcan());
}
} }
} }
delete msg;
} }
delete subscriber;
delete context;
} }
void can_recv_thread(std::vector<Panda *> pandas) { void can_recv_thread(std::vector<Panda *> pandas) {
set_thread_name("boardd_can_recv"); util::set_thread_name("boardd_can_recv");
// can = 8006 // can = 8006
PubMaster pm({"can"}); PubMaster pm({"can"});
@ -415,9 +408,11 @@ void send_peripheral_state(PubMaster *pm, Panda *panda) {
} }
void panda_state_thread(PubMaster *pm, std::vector<Panda *> pandas, bool spoofing_started) { void panda_state_thread(PubMaster *pm, std::vector<Panda *> pandas, bool spoofing_started) {
set_thread_name("boardd_panda_state"); util::set_thread_name("boardd_panda_state");
Params params; Params params;
SubMaster sm({"controlsState"});
Panda *peripheral_panda = pandas[0]; Panda *peripheral_panda = pandas[0];
bool ignition_last = false; bool ignition_last = false;
std::future<bool> safety_future; std::future<bool> safety_future;
@ -452,8 +447,11 @@ void panda_state_thread(PubMaster *pm, std::vector<Panda *> pandas, bool spoofin
ignition_last = ignition; ignition_last = ignition;
sm.update(0);
const bool engaged = sm.allAliveAndValid({"controlsState"}) && sm["controlsState"].getControlsState().getEnabled();
for (const auto &panda : pandas) { for (const auto &panda : pandas) {
panda->send_heartbeat(); panda->send_heartbeat(engaged);
} }
util::sleep_for(500); util::sleep_for(500);
} }
@ -461,7 +459,7 @@ void panda_state_thread(PubMaster *pm, std::vector<Panda *> pandas, bool spoofin
void peripheral_control_thread(Panda *panda) { void peripheral_control_thread(Panda *panda) {
set_thread_name("boardd_peripheral_control"); util::set_thread_name("boardd_peripheral_control");
SubMaster sm({"deviceState", "driverCameraState"}); SubMaster sm({"deviceState", "driverCameraState"});
@ -547,12 +545,12 @@ static void pigeon_publish_raw(PubMaster &pm, const std::string &dat) {
} }
void pigeon_thread(Panda *panda) { void pigeon_thread(Panda *panda) {
set_thread_name("boardd_pigeon"); util::set_thread_name("boardd_pigeon");
PubMaster pm({"ubloxRaw"}); PubMaster pm({"ubloxRaw"});
bool ignition_last = false; bool ignition_last = false;
Pigeon *pigeon = Hardware::TICI() ? Pigeon::connect("/dev/ttyHS0") : Pigeon::connect(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, uint64_t> last_recv_time;
std::unordered_map<char, int64_t> cls_max_dt = { std::unordered_map<char, int64_t> cls_max_dt = {
@ -620,8 +618,6 @@ void pigeon_thread(Panda *panda) {
// 10ms - 100 Hz // 10ms - 100 Hz
util::sleep_for(10); util::sleep_for(10);
} }
delete pigeon;
} }
int main(int argc, char *argv[]) { int main(int argc, char *argv[]) {
@ -629,9 +625,9 @@ int main(int argc, char *argv[]) {
if (!Hardware::PC()) { if (!Hardware::PC()) {
int err; int err;
err = set_realtime_priority(54); err = util::set_realtime_priority(54);
assert(err == 0); assert(err == 0);
err = set_core_affinity({Hardware::TICI() ? 4 : 3}); err = util::set_core_affinity({Hardware::TICI() ? 4 : 3});
assert(err == 0); assert(err == 0);
} }

@ -340,8 +340,8 @@ void Panda::set_usb_power_mode(cereal::PeripheralState::UsbPowerMode power_mode)
usb_write(0xe6, (uint16_t)power_mode, 0); usb_write(0xe6, (uint16_t)power_mode, 0);
} }
void Panda::send_heartbeat() { void Panda::send_heartbeat(bool engaged) {
usb_write(0xf3, 1, 0); usb_write(0xf3, engaged, 0);
} }
void Panda::set_can_speed_kbps(uint16_t bus, uint16_t speed) { void Panda::set_can_speed_kbps(uint16_t bus, uint16_t speed) {

@ -108,7 +108,7 @@ class Panda {
std::optional<std::string> get_serial(); std::optional<std::string> get_serial();
void set_power_saving(bool power_saving); void set_power_saving(bool power_saving);
void set_usb_power_mode(cereal::PeripheralState::UsbPowerMode power_mode); void set_usb_power_mode(cereal::PeripheralState::UsbPowerMode power_mode);
void send_heartbeat(); void send_heartbeat(bool engaged);
void set_can_speed_kbps(uint16_t bus, uint16_t speed); void set_can_speed_kbps(uint16_t bus, uint16_t speed);
void set_data_speed_kbps(uint16_t bus, uint16_t speed); void set_data_speed_kbps(uint16_t bus, uint16_t speed);
void can_send(capnp::List<cereal::CanData>::Reader can_data_list); void can_send(capnp::List<cereal::CanData>::Reader can_data_list);

@ -69,7 +69,7 @@ class TestBoardd(unittest.TestCase):
for __ in range(random.randrange(100)): for __ in range(random.randrange(100)):
bus = random.choice([b for b in range(3*num_pandas) if b % 4 != 3]) bus = random.choice([b for b in range(3*num_pandas) if b % 4 != 3])
addr = random.randrange(1, 1<<29) addr = random.randrange(1, 1<<29)
dat = bytes([random.getrandbits(8) for _ in range(random.randrange(1, 9))]) dat = bytes(random.getrandbits(8) for _ in range(random.randrange(1, 9)))
sent_msgs[bus].add((addr, dat)) sent_msgs[bus].add((addr, dat))
to_send.append(make_can_msg(addr, dat, bus)) to_send.append(make_can_msg(addr, dat, bus))
sendcan.send(can_list_to_can_capnp(to_send, msgtype='sendcan')) sendcan.send(can_list_to_can_capnp(to_send, msgtype='sendcan'))

@ -200,7 +200,6 @@ void fill_frame_data(cereal::FrameData::Builder &framed, const FrameMetadata &fr
framed.setMeasuredGreyFraction(frame_data.measured_grey_fraction); framed.setMeasuredGreyFraction(frame_data.measured_grey_fraction);
framed.setTargetGreyFraction(frame_data.target_grey_fraction); framed.setTargetGreyFraction(frame_data.target_grey_fraction);
framed.setLensPos(frame_data.lens_pos); framed.setLensPos(frame_data.lens_pos);
framed.setLensSag(frame_data.lens_sag);
framed.setLensErr(frame_data.lens_err); framed.setLensErr(frame_data.lens_err);
framed.setLensTruePos(frame_data.lens_true_pos); framed.setLensTruePos(frame_data.lens_true_pos);
} }
@ -351,7 +350,7 @@ void *processing_thread(MultiCameraState *cameras, CameraState *cs, process_thre
} else { } else {
thread_name = "WideRoadCamera"; thread_name = "WideRoadCamera";
} }
set_thread_name(thread_name); util::set_thread_name(thread_name);
uint32_t cnt = 0; uint32_t cnt = 0;
while (!do_exit) { while (!do_exit) {
@ -408,11 +407,11 @@ static void driver_cam_auto_exposure(CameraState *c, SubMaster &sm) {
camera_autoexposure(c, set_exposure_target(b, rect.x1, rect.x2, rect.x_skip, rect.y1, rect.y2, rect.y_skip)); camera_autoexposure(c, set_exposure_target(b, rect.x1, rect.x2, rect.x_skip, rect.y1, rect.y2, rect.y_skip));
} }
void common_process_driver_camera(SubMaster *sm, PubMaster *pm, CameraState *c, int cnt) { void common_process_driver_camera(MultiCameraState *s, CameraState *c, int cnt) {
int j = Hardware::TICI() ? 1 : 3; int j = Hardware::TICI() ? 1 : 3;
if (cnt % j == 0) { if (cnt % j == 0) {
sm->update(0); s->sm->update(0);
driver_cam_auto_exposure(c, *sm); driver_cam_auto_exposure(c, *(s->sm));
} }
MessageBuilder msg; MessageBuilder msg;
auto framed = msg.initEvent().initDriverCameraState(); auto framed = msg.initEvent().initDriverCameraState();
@ -421,5 +420,5 @@ void common_process_driver_camera(SubMaster *sm, PubMaster *pm, CameraState *c,
if (env_send_driver) { if (env_send_driver) {
framed.setImage(get_frame_image(&c->buf)); framed.setImage(get_frame_image(&c->buf));
} }
pm->send("driverCameraState", msg); s->pm->send("driverCameraState", msg);
} }

@ -68,7 +68,6 @@ typedef struct FrameMetadata {
// Focus // Focus
unsigned int lens_pos; unsigned int lens_pos;
float lens_sag;
float lens_err; float lens_err;
float lens_true_pos; float lens_true_pos;
} FrameMetadata; } FrameMetadata;
@ -123,7 +122,7 @@ void fill_frame_data(cereal::FrameData::Builder &framed, const FrameMetadata &fr
kj::Array<uint8_t> get_frame_image(const CameraBuf *b); kj::Array<uint8_t> get_frame_image(const CameraBuf *b);
float set_exposure_target(const CameraBuf *b, int x_start, int x_end, int x_skip, int y_start, int y_end, int y_skip); float set_exposure_target(const CameraBuf *b, int x_start, int x_end, int x_skip, int y_start, int y_end, int y_skip);
std::thread start_process_thread(MultiCameraState *cameras, CameraState *cs, process_thread_cb callback); std::thread start_process_thread(MultiCameraState *cameras, CameraState *cs, process_thread_cb callback);
void common_process_driver_camera(SubMaster *sm, PubMaster *pm, CameraState *c, int cnt); void common_process_driver_camera(MultiCameraState *s, CameraState *c, int cnt);
void cameras_init(VisionIpcServer *v, MultiCameraState *s, cl_device_id device_id, cl_context ctx); void cameras_init(VisionIpcServer *v, MultiCameraState *s, cl_device_id device_id, cl_context ctx);
void cameras_open(MultiCameraState *s); void cameras_open(MultiCameraState *s);

@ -227,7 +227,7 @@ void cameras_init(VisionIpcServer *v, MultiCameraState *s, cl_device_id device_i
s->stats_bufs[i].allocate(0xb80); s->stats_bufs[i].allocate(0xb80);
} }
std::fill_n(s->lapres, std::size(s->lapres), 16160); std::fill_n(s->lapres, std::size(s->lapres), 16160);
s->lap_conv = new LapConv(device_id, ctx, s->road_cam.buf.rgb_width, s->road_cam.buf.rgb_height, 3); s->lap_conv = new LapConv(device_id, ctx, s->road_cam.buf.rgb_width, s->road_cam.buf.rgb_height, s->road_cam.buf.rgb_stride, 3);
} }
static void set_exposure(CameraState *s, float exposure_frac, float gain_frac) { static void set_exposure(CameraState *s, float exposure_frac, float gain_frac) {
@ -852,21 +852,7 @@ static void parse_autofocus(CameraState *s, uint8_t *d) {
s->focus_err = max_focus*1.0; s->focus_err = max_focus*1.0;
} }
static std::optional<float> get_accel_z(SubMaster *sm) { static void do_autofocus(CameraState *s) {
sm->update(0);
if(sm->updated("sensorEvents")) {
for (auto event : (*sm)["sensorEvents"].getSensorEvents()) {
if (event.which() == cereal::SensorEventData::ACCELERATION) {
if (auto v = event.getAcceleration().getV(); v.size() >= 3)
return -v[2];
break;
}
}
}
return std::nullopt;
}
static void do_autofocus(CameraState *s, SubMaster *sm) {
float lens_true_pos = s->lens_true_pos.load(); float lens_true_pos = s->lens_true_pos.load();
if (!isnan(s->focus_err)) { if (!isnan(s->focus_err)) {
// learn lens_true_pos // learn lens_true_pos
@ -874,23 +860,10 @@ static void do_autofocus(CameraState *s, SubMaster *sm) {
lens_true_pos -= s->focus_err*focus_kp; lens_true_pos -= s->focus_err*focus_kp;
} }
if (auto accel_z = get_accel_z(sm)) {
s->last_sag_acc_z = *accel_z;
}
const float sag = (s->last_sag_acc_z / 9.8) * 128;
// stay off the walls // stay off the walls
lens_true_pos = std::clamp(lens_true_pos, float(LP3_AF_DAC_DOWN), float(LP3_AF_DAC_UP)); lens_true_pos = std::clamp(lens_true_pos, float(LP3_AF_DAC_DOWN), float(LP3_AF_DAC_UP));
int target = std::clamp(lens_true_pos - sag, float(LP3_AF_DAC_DOWN), float(LP3_AF_DAC_UP));
s->lens_true_pos.store(lens_true_pos); s->lens_true_pos.store(lens_true_pos);
actuator_move(s, lens_true_pos);
/*char debug[4096];
char *pdebug = debug;
pdebug += sprintf(pdebug, "focus ");
for (int i = 0; i < NUM_FOCUS; i++) pdebug += sprintf(pdebug, "%2x(%4d) ", s->confidence[i], s->focus[i]);
pdebug += sprintf(pdebug, " err: %7.2f offset: %6.2f sag: %6.2f lens_true_pos: %6.2f cur_lens_pos: %4d->%4d", err * focus_kp, offset, sag, s->lens_true_pos, s->cur_lens_pos, target);
LOGD(debug);*/
actuator_move(s, target);
} }
void camera_autoexposure(CameraState *s, float grey_frac) { void camera_autoexposure(CameraState *s, float grey_frac) {
@ -1045,13 +1018,12 @@ static void ops_thread(MultiCameraState *s) {
CameraExpInfo road_cam_op; CameraExpInfo road_cam_op;
CameraExpInfo driver_cam_op; CameraExpInfo driver_cam_op;
set_thread_name("camera_settings"); util::set_thread_name("camera_settings");
SubMaster sm({"sensorEvents"});
while(!do_exit) { while(!do_exit) {
road_cam_op = road_cam_exp.load(); road_cam_op = road_cam_exp.load();
if (road_cam_op.op_id != last_road_cam_op_id) { if (road_cam_op.op_id != last_road_cam_op_id) {
do_autoexposure(&s->road_cam, road_cam_op.grey_frac); do_autoexposure(&s->road_cam, road_cam_op.grey_frac);
do_autofocus(&s->road_cam, &sm); do_autofocus(&s->road_cam);
last_road_cam_op_id = road_cam_op.op_id; last_road_cam_op_id = road_cam_op.op_id;
} }
@ -1086,10 +1058,6 @@ static void setup_self_recover(CameraState *c, const uint16_t *lapres, size_t la
c->self_recover.store(self_recover); c->self_recover.store(self_recover);
} }
void process_driver_camera(MultiCameraState *s, CameraState *c, int cnt) {
common_process_driver_camera(s->sm, s->pm, c, cnt);
}
// called by processing_thread // called by processing_thread
void process_road_camera(MultiCameraState *s, CameraState *c, int cnt) { void process_road_camera(MultiCameraState *s, CameraState *c, int cnt) {
const CameraBuf *b = &c->buf; const CameraBuf *b = &c->buf;
@ -1121,7 +1089,7 @@ void cameras_run(MultiCameraState *s) {
std::vector<std::thread> threads; std::vector<std::thread> threads;
threads.push_back(std::thread(ops_thread, s)); threads.push_back(std::thread(ops_thread, s));
threads.push_back(start_process_thread(s, &s->road_cam, process_road_camera)); threads.push_back(start_process_thread(s, &s->road_cam, process_road_camera));
threads.push_back(start_process_thread(s, &s->driver_cam, process_driver_camera)); threads.push_back(start_process_thread(s, &s->driver_cam, common_process_driver_camera));
CameraState* cameras[2] = {&s->road_cam, &s->driver_cam}; CameraState* cameras[2] = {&s->road_cam, &s->driver_cam};
@ -1169,7 +1137,6 @@ void cameras_run(MultiCameraState *s) {
.frame_length = (uint32_t)c->frame_length, .frame_length = (uint32_t)c->frame_length,
.integ_lines = (uint32_t)c->cur_integ_lines, .integ_lines = (uint32_t)c->cur_integ_lines,
.lens_pos = c->cur_lens_pos, .lens_pos = c->cur_lens_pos,
.lens_sag = c->last_sag_acc_z,
.lens_err = c->focus_err, .lens_err = c->focus_err,
.lens_true_pos = c->lens_true_pos, .lens_true_pos = c->lens_true_pos,
.gain = c->cur_gain_frac, .gain = c->cur_gain_frac,

@ -76,7 +76,6 @@ typedef struct CameraState {
// rear camera only,used for focusing // rear camera only,used for focusing
unique_fd actuator_fd; unique_fd actuator_fd;
std::atomic<float> focus_err; std::atomic<float> focus_err;
std::atomic<float> last_sag_acc_z;
std::atomic<float> lens_true_pos; std::atomic<float> lens_true_pos;
std::atomic<int> self_recover; // af recovery counter, neg is patience, pos is active std::atomic<int> self_recover; // af recovery counter, neg is patience, pos is active
uint16_t cur_step_pos; uint16_t cur_step_pos;

@ -987,11 +987,6 @@ void camera_autoexposure(CameraState *s, float grey_frac) {
set_camera_exposure(s, grey_frac); set_camera_exposure(s, grey_frac);
} }
void process_driver_camera(MultiCameraState *s, CameraState *c, int cnt) {
common_process_driver_camera(s->sm, s->pm, c, cnt);
}
// called by processing_thread // called by processing_thread
void process_road_camera(MultiCameraState *s, CameraState *c, int cnt) { void process_road_camera(MultiCameraState *s, CameraState *c, int cnt) {
const CameraBuf *b = &c->buf; const CameraBuf *b = &c->buf;
@ -1016,7 +1011,7 @@ void cameras_run(MultiCameraState *s) {
LOG("-- Starting threads"); LOG("-- Starting threads");
std::vector<std::thread> 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->road_cam, process_road_camera));
threads.push_back(start_process_thread(s, &s->driver_cam, process_driver_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)); threads.push_back(start_process_thread(s, &s->wide_road_cam, process_road_camera));
// start devices // start devices

@ -67,12 +67,12 @@ void run_camera(CameraState *s) {
} }
void road_camera_thread(CameraState *s) { void road_camera_thread(CameraState *s) {
set_thread_name("replay_road_camera_thread"); util::set_thread_name("replay_road_camera_thread");
run_camera(s); run_camera(s);
} }
// void driver_camera_thread(CameraState *s) { // void driver_camera_thread(CameraState *s) {
// set_thread_name("replay_driver_camera_thread"); // util::set_thread_name("replay_driver_camera_thread");
// run_camera(s); // run_camera(s);
// } // }

@ -97,7 +97,7 @@ void run_camera(CameraState *s, cv::VideoCapture &video_cap, float *ts) {
} }
static void road_camera_thread(CameraState *s) { static void road_camera_thread(CameraState *s) {
set_thread_name("webcam_road_camera_thread"); util::set_thread_name("webcam_road_camera_thread");
cv::VideoCapture cap_road(ROAD_CAMERA_ID, cv::CAP_V4L2); // road cv::VideoCapture cap_road(ROAD_CAMERA_ID, cv::CAP_V4L2); // road
cap_road.set(cv::CAP_PROP_FRAME_WIDTH, 853); cap_road.set(cv::CAP_PROP_FRAME_WIDTH, 853);
@ -186,7 +186,7 @@ void cameras_run(MultiCameraState *s) {
threads.push_back(start_process_thread(s, &s->driver_cam, process_driver_camera)); threads.push_back(start_process_thread(s, &s->driver_cam, process_driver_camera));
std::thread t_rear = std::thread(road_camera_thread, &s->road_cam); std::thread t_rear = std::thread(road_camera_thread, &s->road_cam);
set_thread_name("webcam_thread"); util::set_thread_name("webcam_thread");
driver_camera_thread(&s->driver_cam); driver_camera_thread(&s->driver_cam);
t_rear.join(); t_rear.join();

@ -55,8 +55,8 @@ static cl_program build_conv_program(cl_device_id device_id, cl_context context,
return cl_program_from_file(context, device_id, "imgproc/conv.cl", args); return cl_program_from_file(context, device_id, "imgproc/conv.cl", args);
} }
LapConv::LapConv(cl_device_id device_id, cl_context ctx, int rgb_width, int rgb_height, int filter_size) LapConv::LapConv(cl_device_id device_id, cl_context ctx, int rgb_width, int rgb_height, int rgb_stride, int filter_size)
: width(rgb_width / NUM_SEGMENTS_X), height(rgb_height / NUM_SEGMENTS_Y), : width(rgb_width / NUM_SEGMENTS_X), height(rgb_height / NUM_SEGMENTS_Y), rgb_stride(rgb_stride),
roi_buf(width * height * 3), result_buf(width * height) { roi_buf(width * height * 3), result_buf(width * height) {
prg = build_conv_program(device_id, ctx, width, height, filter_size); prg = build_conv_program(device_id, ctx, width, height, filter_size);
@ -81,9 +81,9 @@ uint16_t LapConv::Update(cl_command_queue q, const uint8_t *rgb_buf, const int r
const int x_offset = ROI_X_MIN + roi_id % (ROI_X_MAX - ROI_X_MIN + 1); const int x_offset = ROI_X_MIN + roi_id % (ROI_X_MAX - ROI_X_MIN + 1);
const int y_offset = ROI_Y_MIN + roi_id / (ROI_X_MAX - ROI_X_MIN + 1); const int y_offset = ROI_Y_MIN + roi_id / (ROI_X_MAX - ROI_X_MIN + 1);
const uint8_t *rgb_offset = rgb_buf + y_offset * height * FULL_STRIDE_X * 3 + x_offset * width * 3; const uint8_t *rgb_offset = rgb_buf + y_offset * height * rgb_stride + x_offset * width * 3;
for (int i = 0; i < height; ++i) { for (int i = 0; i < height; ++i) {
memcpy(&roi_buf[i * width * 3], &rgb_offset[i * FULL_STRIDE_X * 3], width * 3); memcpy(&roi_buf[i * width * 3], &rgb_offset[i * rgb_stride], width * 3);
} }
constexpr int local_mem_size = (CONV_LOCAL_WORKSIZE + 2 * (3 / 2)) * (CONV_LOCAL_WORKSIZE + 2 * (3 / 2)) * (3 * sizeof(uint8_t)); constexpr int local_mem_size = (CONV_LOCAL_WORKSIZE + 2 * (3 / 2)) * (CONV_LOCAL_WORKSIZE + 2 * (3 / 2)) * (3 * sizeof(uint8_t));

@ -16,16 +16,11 @@
#define LM_THRESH 120 #define LM_THRESH 120
#define LM_PREC_THRESH 0.9 // 90 perc is blur #define LM_PREC_THRESH 0.9 // 90 perc is blur
// only apply to QCOM
#define FULL_STRIDE_X 1280
#define FULL_STRIDE_Y 896
#define CONV_LOCAL_WORKSIZE 16 #define CONV_LOCAL_WORKSIZE 16
class LapConv { class LapConv {
public: public:
LapConv(cl_device_id device_id, cl_context ctx, int rgb_width, int rgb_height, int filter_size); LapConv(cl_device_id device_id, cl_context ctx, int rgb_width, int rgb_height, int rgb_stride, int filter_size);
~LapConv(); ~LapConv();
uint16_t Update(cl_command_queue q, const uint8_t *rgb_buf, const int roi_id); uint16_t Update(cl_command_queue q, const uint8_t *rgb_buf, const int roi_id);
@ -34,6 +29,7 @@ private:
cl_program prg; cl_program prg;
cl_kernel krnl; cl_kernel krnl;
const int width, height; const int width, height;
const int rgb_stride;
std::vector<uint8_t> roi_buf; std::vector<uint8_t> roi_buf;
std::vector<int16_t> result_buf; std::vector<int16_t> result_buf;
}; };

@ -46,9 +46,9 @@ void party(cl_device_id device_id, cl_context context) {
int main(int argc, char *argv[]) { int main(int argc, char *argv[]) {
if (!Hardware::PC()) { if (!Hardware::PC()) {
int ret; int ret;
ret = set_realtime_priority(53); ret = util::set_realtime_priority(53);
assert(ret == 0); assert(ret == 0);
ret = set_core_affinity({Hardware::EON() ? 2 : 6}); ret = util::set_core_affinity({Hardware::EON() ? 2 : 6});
assert(ret == 0 || Params().getBool("IsOffroad")); // failure ok while offroad due to offlining cores assert(ret == 0 || Params().getBool("IsOffroad")); // failure ok while offroad due to offlining cores
} }

@ -54,7 +54,7 @@ class TestCamerad(unittest.TestCase):
self.assertTrue(abs(dfid - 1) <= SKIP_FRAME_TOLERANCE, "%s frame id diff is %d" % (camera, dfid)) self.assertTrue(abs(dfid - 1) <= SKIP_FRAME_TOLERANCE, "%s frame id diff is %d" % (camera, dfid))
dts = ct - last_ts[camera] dts = ct - last_ts[camera]
self.assertTrue(abs(dts - (1000/CAMERAS[camera])) < LAG_FRAME_TOLERANCE, "%s frame t(ms) diff is %f" % (camera, dts)) self.assertTrue(abs(dts - (1000/CAMERAS[camera])) < LAG_FRAME_TOLERANCE, f"{camera} frame t(ms) diff is {dts:f}")
last_frame_id[camera] = sm[camera].frameId last_frame_id[camera] = sm[camera].frameId
last_ts[camera] = ct last_ts[camera] = ct

@ -1,7 +1,7 @@
import os import os
from common.params import Params from common.params import Params
from common.basedir import BASEDIR from common.basedir import BASEDIR
from selfdrive.version import get_comma_remote, get_tested_branch from selfdrive.version import is_comma_remote, is_tested_branch
from selfdrive.car.fingerprints import eliminate_incompatible_cars, all_legacy_fingerprint_cars from selfdrive.car.fingerprints import eliminate_incompatible_cars, all_legacy_fingerprint_cars
from selfdrive.car.vin import get_vin, VIN_UNKNOWN from selfdrive.car.vin import get_vin, VIN_UNKNOWN
from selfdrive.car.fw_versions import get_fw_versions, match_fw_to_car from selfdrive.car.fw_versions import get_fw_versions, match_fw_to_car
@ -14,7 +14,7 @@ EventName = car.CarEvent.EventName
def get_startup_event(car_recognized, controller_available, fw_seen): def get_startup_event(car_recognized, controller_available, fw_seen):
if get_comma_remote() and get_tested_branch(): if is_comma_remote() and is_tested_branch():
event = EventName.startup event = EventName.startup
else: else:
event = EventName.startupMaster event = EventName.startupMaster
@ -39,7 +39,7 @@ def get_one_can(logcan):
def load_interfaces(brand_names): def load_interfaces(brand_names):
ret = {} ret = {}
for brand_name in brand_names: for brand_name in brand_names:
path = ('selfdrive.car.%s' % brand_name) path = f'selfdrive.car.{brand_name}'
CarInterface = __import__(path + '.interface', fromlist=['CarInterface']).CarInterface CarInterface = __import__(path + '.interface', fromlist=['CarInterface']).CarInterface
if os.path.exists(BASEDIR + '/' + path.replace('.', '/') + '/carstate.py'): if os.path.exists(BASEDIR + '/' + path.replace('.', '/') + '/carstate.py'):
@ -65,10 +65,10 @@ def _get_interface_names():
for car_folder in [x[0] for x in os.walk(BASEDIR + '/selfdrive/car')]: for car_folder in [x[0] for x in os.walk(BASEDIR + '/selfdrive/car')]:
try: try:
brand_name = car_folder.split('/')[-1] brand_name = car_folder.split('/')[-1]
model_names = __import__('selfdrive.car.%s.values' % brand_name, fromlist=['CAR']).CAR model_names = __import__(f'selfdrive.car.{brand_name}.values', fromlist=['CAR']).CAR
model_names = [getattr(model_names, c) for c in model_names.__dict__.keys() if not c.startswith("__")] model_names = [getattr(model_names, c) for c in model_names.__dict__.keys() if not c.startswith("__")]
brand_names[brand_name] = model_names brand_names[brand_name] = model_names
except (ImportError, IOError): except (ImportError, OSError):
pass pass
return brand_names return brand_names

@ -1,3 +1,4 @@
from cereal import car
from selfdrive.car import apply_toyota_steer_torque_limits from selfdrive.car import apply_toyota_steer_torque_limits
from selfdrive.car.chrysler.chryslercan import create_lkas_hud, create_lkas_command, \ from selfdrive.car.chrysler.chryslercan import create_lkas_hud, create_lkas_command, \
create_wheel_buttons create_wheel_buttons
@ -20,9 +21,8 @@ class CarController():
# this seems needed to avoid steering faults and to force the sync with the EPS counter # this seems needed to avoid steering faults and to force the sync with the EPS counter
frame = CS.lkas_counter frame = CS.lkas_counter
if self.prev_frame == frame: if self.prev_frame == frame:
return [] return car.CarControl.Actuators.new_message(), []
# *** compute control surfaces ***
# steer torque # steer torque
new_steer = int(round(actuators.steer * CarControllerParams.STEER_MAX)) new_steer = int(round(actuators.steer * CarControllerParams.STEER_MAX))
apply_steer = apply_toyota_steer_torque_limits(new_steer, self.apply_steer_last, apply_steer = apply_toyota_steer_torque_limits(new_steer, self.apply_steer_last,
@ -67,4 +67,7 @@ class CarController():
self.ccframe += 1 self.ccframe += 1
self.prev_frame = frame self.prev_frame = frame
return can_sends new_actuators = actuators.copy()
new_actuators.steer = apply_steer / CarControllerParams.STEER_MAX
return new_actuators, can_sends

@ -78,8 +78,6 @@ class CarInterface(CarInterfaceBase):
def apply(self, c): def apply(self, c):
if (self.CS.frame == -1): if (self.CS.frame == -1):
return [] # if we haven't seen a frame 220, then do not update. return car.CarControl.Actuators.new_message(), [] # if we haven't seen a frame 220, then do not update.
can_sends = self.CC.update(c.enabled, self.CS, c.actuators, c.cruiseControl.cancel, c.hudControl.visualAlert) return self.CC.update(c.enabled, self.CS, c.actuators, c.cruiseControl.cancel, c.hudControl.visualAlert)
return can_sends

@ -11,7 +11,7 @@ def get_attr_from_cars(attr, result=dict, combine_brands=True):
for car_folder in [x[0] for x in os.walk(BASEDIR + '/selfdrive/car')]: for car_folder in [x[0] for x in os.walk(BASEDIR + '/selfdrive/car')]:
try: try:
car_name = car_folder.split('/')[-1] car_name = car_folder.split('/')[-1]
values = __import__('selfdrive.car.%s.values' % car_name, fromlist=[attr]) values = __import__(f'selfdrive.car.{car_name}.values', fromlist=[attr])
if hasattr(values, attr): if hasattr(values, attr):
attr_values = getattr(values, attr) attr_values = getattr(values, attr)
else: else:
@ -28,7 +28,7 @@ def get_attr_from_cars(attr, result=dict, combine_brands=True):
elif isinstance(attr_values, list): elif isinstance(attr_values, list):
result += attr_values result += attr_values
except (ImportError, IOError): except (ImportError, OSError):
pass pass
return result return result

@ -32,7 +32,7 @@ class CarController():
if (frame % 3) == 0: if (frame % 3) == 0:
curvature = self.vehicle_model.calc_curvature(actuators.steeringAngleDeg*math.pi/180., CS.out.vEgo) curvature = self.vehicle_model.calc_curvature(math.radians(actuators.steeringAngleDeg), CS.out.vEgo, 0.0)
# The use of the toggle below is handy for trying out the various LKAS modes # The use of the toggle below is handy for trying out the various LKAS modes
if TOGGLE_DEBUG: if TOGGLE_DEBUG:
@ -83,4 +83,4 @@ class CarController():
self.main_on_last = CS.out.cruiseState.available self.main_on_last = CS.out.cruiseState.available
self.steer_alert_last = steer_alert self.steer_alert_last = steer_alert
return can_sends return actuators, can_sends

@ -63,8 +63,8 @@ class CarInterface(CarInterfaceBase):
# to be called @ 100hz # to be called @ 100hz
def apply(self, c): def apply(self, c):
can_sends = self.CC.update(c.enabled, self.CS, self.frame, c.actuators, ret = self.CC.update(c.enabled, self.CS, self.frame, c.actuators,
c.hudControl.visualAlert, c.cruiseControl.cancel) c.hudControl.visualAlert, c.cruiseControl.cancel)
self.frame += 1 self.frame += 1
return can_sends return ret

@ -84,8 +84,21 @@ NISSAN_VERSION_RESPONSE_STANDARD = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIF
NISSAN_RX_OFFSET = 0x20 NISSAN_RX_OFFSET = 0x20
SUBARU_VERSION_REQUEST = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \
p16(uds.DATA_IDENTIFIER_TYPE.APPLICATION_DATA_IDENTIFICATION)
SUBARU_VERSION_RESPONSE = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40]) + \
p16(uds.DATA_IDENTIFIER_TYPE.APPLICATION_DATA_IDENTIFICATION)
# brand, request, response, response offset # brand, request, response, response offset
REQUESTS = [ REQUESTS = [
# Subaru
(
"subaru",
[TESTER_PRESENT_REQUEST, SUBARU_VERSION_REQUEST],
[TESTER_PRESENT_RESPONSE, SUBARU_VERSION_RESPONSE],
DEFAULT_RX_OFFSET,
),
# Hyundai # Hyundai
( (
"hyundai", "hyundai",
@ -221,7 +234,7 @@ def match_fw_to_car_fuzzy(fw_versions_dict, log=True, exclude=None):
if match_count >= 2: if match_count >= 2:
if log: if log:
cloudlog.error(f"Fingerprinted {candidate} using fuzzy match. {match_count} matching ECUs") cloudlog.error(f"Fingerprinted {candidate} using fuzzy match. {match_count} matching ECUs")
return set([candidate]) return {candidate}
else: else:
return set() return set()
@ -362,7 +375,7 @@ if __name__ == "__main__":
print("Getting vin...") print("Getting vin...")
addr, vin = get_vin(logcan, sendcan, 1, retry=10, debug=args.debug) addr, vin = get_vin(logcan, sendcan, 1, retry=10, debug=args.debug)
print(f"VIN: {vin}") print(f"VIN: {vin}")
print("Getting VIN took %.3f s" % (time.time() - t)) print(f"Getting VIN took {time.time() - t:.3f} s")
print() print()
t = time.time() t = time.time()
@ -379,4 +392,4 @@ if __name__ == "__main__":
print() print()
print("Possible matches:", candidates) print("Possible matches:", candidates)
print("Getting fw took %.3f s" % (time.time() - t)) print(f"Getting fw took {time.time() - t:.3f} s")

@ -14,6 +14,9 @@ class CarController():
def __init__(self, dbc_name, CP, VM): def __init__(self, dbc_name, CP, VM):
self.start_time = 0. self.start_time = 0.
self.apply_steer_last = 0 self.apply_steer_last = 0
self.apply_gas = 0
self.apply_brake = 0
self.lka_steering_cmd_counter_last = -1 self.lka_steering_cmd_counter_last = -1
self.lka_icon_status_last = (False, False) self.lka_icon_status_last = (False, False)
self.steer_rate_limited = False self.steer_rate_limited = False
@ -53,22 +56,22 @@ class CarController():
can_sends.append(gmcan.create_steering_control(self.packer_pt, CanBus.POWERTRAIN, apply_steer, idx, lkas_enabled)) can_sends.append(gmcan.create_steering_control(self.packer_pt, CanBus.POWERTRAIN, apply_steer, idx, lkas_enabled))
if not enabled:
# Stock ECU sends max regen when not enabled.
apply_gas = P.MAX_ACC_REGEN
apply_brake = 0
else:
apply_gas = int(round(interp(actuators.accel, P.GAS_LOOKUP_BP, P.GAS_LOOKUP_V)))
apply_brake = int(round(interp(actuators.accel, P.BRAKE_LOOKUP_BP, P.BRAKE_LOOKUP_V)))
# Gas/regen and brakes - all at 25Hz # Gas/regen and brakes - all at 25Hz
if (frame % 4) == 0: if (frame % 4) == 0:
if not enabled:
# Stock ECU sends max regen when not enabled.
self.apply_gas = P.MAX_ACC_REGEN
self.apply_brake = 0
else:
self.apply_gas = int(round(interp(actuators.accel, P.GAS_LOOKUP_BP, P.GAS_LOOKUP_V)))
self.apply_brake = int(round(interp(actuators.accel, P.BRAKE_LOOKUP_BP, P.BRAKE_LOOKUP_V)))
idx = (frame // 4) % 4 idx = (frame // 4) % 4
at_full_stop = enabled and CS.out.standstill at_full_stop = enabled and CS.out.standstill
near_stop = enabled and (CS.out.vEgo < P.NEAR_STOP_BRAKE_PHASE) near_stop = enabled and (CS.out.vEgo < P.NEAR_STOP_BRAKE_PHASE)
can_sends.append(gmcan.create_friction_brake_command(self.packer_ch, CanBus.CHASSIS, apply_brake, idx, near_stop, at_full_stop)) can_sends.append(gmcan.create_friction_brake_command(self.packer_ch, CanBus.CHASSIS, self.apply_brake, idx, near_stop, at_full_stop))
can_sends.append(gmcan.create_gas_regen_command(self.packer_pt, CanBus.POWERTRAIN, apply_gas, idx, enabled, at_full_stop)) can_sends.append(gmcan.create_gas_regen_command(self.packer_pt, CanBus.POWERTRAIN, self.apply_gas, idx, enabled, at_full_stop))
# Send dashboard UI commands (ACC status), 25hz # Send dashboard UI commands (ACC status), 25hz
if (frame % 4) == 0: if (frame % 4) == 0:
@ -106,4 +109,9 @@ class CarController():
can_sends.append(gmcan.create_lka_icon_command(CanBus.SW_GMLAN, lka_active, lka_critical, steer_alert)) can_sends.append(gmcan.create_lka_icon_command(CanBus.SW_GMLAN, lka_active, lka_critical, steer_alert))
self.lka_icon_status_last = lka_icon_status self.lka_icon_status_last = lka_icon_status
return can_sends new_actuators = actuators.copy()
new_actuators.steer = self.apply_steer_last / P.STEER_MAX
new_actuators.gas = self.apply_gas
new_actuators.brake = self.apply_brake
return new_actuators, can_sends

@ -36,7 +36,7 @@ class CarState(CarStateBase):
if ret.brake < 10/0xd0: if ret.brake < 10/0xd0:
ret.brake = 0. ret.brake = 0.
ret.gas = pt_cp.vl["AcceleratorPedal"]["AcceleratorPedal"] / 254. ret.gas = pt_cp.vl["AcceleratorPedal2"]["AcceleratorPedal2"] / 254.
ret.gasPressed = ret.gas > 1e-5 ret.gasPressed = ret.gas > 1e-5
ret.steeringAngleDeg = pt_cp.vl["PSCMSteeringAngle"]["SteeringWheelAngle"] ret.steeringAngleDeg = pt_cp.vl["PSCMSteeringAngle"]["SteeringWheelAngle"]
@ -90,7 +90,7 @@ class CarState(CarStateBase):
("LeftSeatBelt", "BCMDoorBeltStatus", 0), ("LeftSeatBelt", "BCMDoorBeltStatus", 0),
("RightSeatBelt", "BCMDoorBeltStatus", 0), ("RightSeatBelt", "BCMDoorBeltStatus", 0),
("TurnSignals", "BCMTurnSignals", 0), ("TurnSignals", "BCMTurnSignals", 0),
("AcceleratorPedal", "AcceleratorPedal", 0), ("AcceleratorPedal2", "AcceleratorPedal2", 0),
("CruiseState", "AcceleratorPedal2", 0), ("CruiseState", "AcceleratorPedal2", 0),
("ACCButtons", "ASCMSteeringButton", CruiseButtons.UNPRESS), ("ACCButtons", "ASCMSteeringButton", CruiseButtons.UNPRESS),
("SteeringWheelAngle", "PSCMSteeringAngle", 0), ("SteeringWheelAngle", "PSCMSteeringAngle", 0),
@ -117,7 +117,6 @@ class CarState(CarStateBase):
("EPBStatus", 20), ("EPBStatus", 20),
("EBCMWheelSpdFront", 20), ("EBCMWheelSpdFront", 20),
("EBCMWheelSpdRear", 20), ("EBCMWheelSpdRear", 20),
("AcceleratorPedal", 33),
("AcceleratorPedal2", 33), ("AcceleratorPedal2", 33),
("ASCMSteeringButton", 33), ("ASCMSteeringButton", 33),
("ECMEngineStatus", 100), ("ECMEngineStatus", 100),

@ -212,7 +212,8 @@ class CarInterface(CarInterfaceBase):
return self.CS.out return self.CS.out
def apply(self, c): def apply(self, c):
hud_v_cruise = c.hudControl.setSpeed hud_control = c.hudControl
hud_v_cruise = hud_control.setSpeed
if hud_v_cruise > 70: if hud_v_cruise > 70:
hud_v_cruise = 0 hud_v_cruise = 0
@ -220,10 +221,10 @@ class CarInterface(CarInterfaceBase):
# In GM, PCM faults out if ACC command overlaps user gas. # In GM, PCM faults out if ACC command overlaps user gas.
enabled = c.enabled and not self.CS.out.gasPressed enabled = c.enabled and not self.CS.out.gasPressed
can_sends = self.CC.update(enabled, self.CS, self.frame, ret = self.CC.update(enabled, self.CS, self.frame,
c.actuators, c.actuators,
hud_v_cruise, c.hudControl.lanesVisible, hud_v_cruise, hud_control.lanesVisible,
c.hudControl.leadVisible, c.hudControl.visualAlert) hud_control.leadVisible, hud_control.visualAlert)
self.frame += 1 self.frame += 1
return can_sends return ret

@ -1,5 +1,4 @@
#!/usr/bin/env python3 #!/usr/bin/env python3
from __future__ import print_function
import math import math
from cereal import car from cereal import car
from opendbc.can.parser import CANParser from opendbc.can.parser import CANParser

@ -105,6 +105,11 @@ class CarController():
self.last_pump_ts = 0. self.last_pump_ts = 0.
self.packer = CANPacker(dbc_name) self.packer = CANPacker(dbc_name)
self.accel = 0
self.speed = 0
self.gas = 0
self.brake = 0
self.params = CarControllerParams(CP) self.params = CarControllerParams(CP)
def update(self, enabled, active, CS, frame, actuators, pcm_cancel_cmd, def update(self, enabled, active, CS, frame, actuators, pcm_cancel_cmd,
@ -211,10 +216,9 @@ class CarController():
ts = frame * DT_CTRL ts = frame * DT_CTRL
if CS.CP.carFingerprint in HONDA_BOSCH: if CS.CP.carFingerprint in HONDA_BOSCH:
accel = clip(accel, P.BOSCH_ACCEL_MIN, P.BOSCH_ACCEL_MAX) self.accel = clip(accel, P.BOSCH_ACCEL_MIN, P.BOSCH_ACCEL_MAX)
bosch_gas = interp(accel, P.BOSCH_GAS_LOOKUP_BP, P.BOSCH_GAS_LOOKUP_V) self.gas = interp(accel, P.BOSCH_GAS_LOOKUP_BP, P.BOSCH_GAS_LOOKUP_V)
can_sends.extend(hondacan.create_acc_commands(self.packer, enabled, active, accel, bosch_gas, idx, stopping, starting, CS.CP.carFingerprint)) can_sends.extend(hondacan.create_acc_commands(self.packer, enabled, active, accel, self.gas, idx, stopping, starting, CS.CP.carFingerprint))
else: else:
apply_brake = clip(self.brake_last - wind_brake, 0.0, 1.0) apply_brake = clip(self.brake_last - wind_brake, 0.0, 1.0)
apply_brake = int(clip(apply_brake * P.NIDEC_BRAKE_MAX, 0, P.NIDEC_BRAKE_MAX - 1)) apply_brake = int(clip(apply_brake * P.NIDEC_BRAKE_MAX, 0, P.NIDEC_BRAKE_MAX - 1))
@ -224,6 +228,7 @@ class CarController():
can_sends.append(hondacan.create_brake_command(self.packer, apply_brake, pump_on, can_sends.append(hondacan.create_brake_command(self.packer, apply_brake, pump_on,
pcm_override, pcm_cancel_cmd, fcw_display, idx, CS.CP.carFingerprint, CS.stock_brake)) pcm_override, pcm_cancel_cmd, fcw_display, idx, CS.CP.carFingerprint, CS.stock_brake))
self.apply_brake_last = apply_brake self.apply_brake_last = apply_brake
self.brake = apply_brake / P.NIDEC_BRAKE_MAX
if CS.CP.enableGasInterceptor: if CS.CP.enableGasInterceptor:
# way too aggressive at low speed without this # way too aggressive at low speed without this
@ -233,17 +238,28 @@ class CarController():
# Sending non-zero gas when OP is not enabled will cause the PCM not to respond to throttle as expected # Sending non-zero gas when OP is not enabled will cause the PCM not to respond to throttle as expected
# when you do enable. # when you do enable.
if active: if active:
apply_gas = clip(gas_mult * (gas - brake + wind_brake*3/4), 0., 1.) self.gas = clip(gas_mult * (gas - brake + wind_brake*3/4), 0., 1.)
else: else:
apply_gas = 0.0 self.gas = 0.0
can_sends.append(create_gas_interceptor_command(self.packer, apply_gas, idx)) can_sends.append(create_gas_interceptor_command(self.packer, self.gas, idx))
hud = HUDData(int(pcm_accel), int(round(hud_v_cruise)), hud_car,
hud_lanes, fcw_display, acc_alert, steer_required)
# Send dashboard UI commands. # Send dashboard UI commands.
if (frame % 10) == 0: if (frame % 10) == 0:
idx = (frame//10) % 4 idx = (frame//10) % 4
hud = HUDData(int(pcm_accel), int(round(hud_v_cruise)), hud_car,
hud_lanes, fcw_display, acc_alert, steer_required)
can_sends.extend(hondacan.create_ui_commands(self.packer, CS.CP, pcm_speed, hud, CS.is_metric, idx, CS.stock_hud)) can_sends.extend(hondacan.create_ui_commands(self.packer, CS.CP, pcm_speed, hud, CS.is_metric, idx, CS.stock_hud))
return can_sends if (CS.CP.openpilotLongitudinalControl) and (CS.CP.carFingerprint not in HONDA_BOSCH):
self.speed = pcm_speed
if not CS.CP.enableGasInterceptor:
self.gas = pcm_accel / 0xc6
new_actuators = actuators.copy()
new_actuators.speed = self.speed
new_actuators.accel = self.accel
new_actuators.gas = self.gas
new_actuators.brake = self.brake
return new_actuators, can_sends

@ -227,7 +227,7 @@ class CarInterface(CarInterfaceBase):
ret.centerToFront = ret.wheelbase * 0.41 ret.centerToFront = ret.wheelbase * 0.41
ret.steerRatio = 11.95 # as spec ret.steerRatio = 11.95 # as spec
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 3840], [0, 3840]] ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 3840], [0, 3840]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.18]] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2], [0.06]]
tire_stiffness_factor = 0.677 tire_stiffness_factor = 0.677
elif candidate == CAR.ODYSSEY: elif candidate == CAR.ODYSSEY:
@ -301,7 +301,7 @@ class CarInterface(CarInterfaceBase):
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.18]] # TODO: can probably use some tuning ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.18]] # TODO: can probably use some tuning
else: else:
raise ValueError("unsupported car %s" % candidate) raise ValueError(f"unsupported car {candidate}")
# These cars use alternate user brake msg (0x1BE) # These cars use alternate user brake msg (0x1BE)
if candidate in HONDA_BOSCH_ALT_BRAKE_SIGNAL: if candidate in HONDA_BOSCH_ALT_BRAKE_SIGNAL:
@ -350,7 +350,6 @@ class CarInterface(CarInterfaceBase):
ret = self.CS.update(self.cp, self.cp_cam, self.cp_body) ret = self.CS.update(self.cp, self.cp_cam, self.cp_body)
ret.canValid = self.cp.can_valid and self.cp_cam.can_valid and (self.cp_body is None or self.cp_body.can_valid) ret.canValid = self.cp.can_valid and self.cp_cam.can_valid and (self.cp_body is None or self.cp_body.can_valid)
ret.yawRate = self.VM.yaw_rate(ret.steeringAngleDeg * CV.DEG_TO_RAD, ret.vEgo)
buttonEvents = [] buttonEvents = []
@ -433,18 +432,19 @@ class CarInterface(CarInterfaceBase):
# pass in a car.CarControl # pass in a car.CarControl
# to be called @ 100hz # to be called @ 100hz
def apply(self, c): def apply(self, c):
if c.hudControl.speedVisible: hud_control = c.hudControl
hud_v_cruise = c.hudControl.setSpeed * CV.MS_TO_KPH if hud_control.speedVisible:
hud_v_cruise = hud_control.setSpeed * CV.MS_TO_KPH
else: else:
hud_v_cruise = 255 hud_v_cruise = 255
can_sends = self.CC.update(c.enabled, c.active, self.CS, self.frame, ret = self.CC.update(c.enabled, c.active, self.CS, self.frame,
c.actuators, c.actuators,
c.cruiseControl.cancel, c.cruiseControl.cancel,
hud_v_cruise, hud_v_cruise,
c.hudControl.lanesVisible, hud_control.lanesVisible,
hud_show_car=c.hudControl.leadVisible, hud_show_car=hud_control.leadVisible,
hud_alert=c.hudControl.visualAlert) hud_alert=hud_control.visualAlert)
self.frame += 1 self.frame += 1
return can_sends return ret

@ -1385,9 +1385,9 @@ STEER_THRESHOLD = {
CAR.CRV_EU: 400, CAR.CRV_EU: 400,
} }
HONDA_NIDEC_ALT_PCM_ACCEL = set([CAR.ODYSSEY]) HONDA_NIDEC_ALT_PCM_ACCEL = {CAR.ODYSSEY}
HONDA_NIDEC_ALT_SCM_MESSAGES = set([CAR.ACURA_ILX, CAR.ACURA_RDX, CAR.CRV, CAR.CRV_EU, CAR.FIT, CAR.FREED, CAR.HRV, CAR.ODYSSEY_CHN, 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.PILOT_2019, CAR.PASSPORT, CAR.RIDGELINE}
HONDA_BOSCH = set([CAR.ACCORD, CAR.ACCORDH, CAR.CIVIC_BOSCH, CAR.CIVIC_BOSCH_DIESEL, CAR.CRV_5G, 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 = set([CAR.ACCORD, CAR.CRV_5G, CAR.ACURA_RDX_3G]) HONDA_BOSCH_ALT_BRAKE_SIGNAL = {CAR.ACCORD, CAR.CRV_5G, CAR.ACURA_RDX_3G}

@ -44,6 +44,7 @@ class CarController():
self.car_fingerprint = CP.carFingerprint self.car_fingerprint = CP.carFingerprint
self.steer_rate_limited = False self.steer_rate_limited = False
self.last_resume_frame = 0 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, enabled, CS, frame, actuators, pcm_cancel_cmd, visual_alert, hud_speed,
left_lane, right_lane, left_lane_depart, right_lane_depart): left_lane, right_lane, left_lane_depart, right_lane_depart):
@ -100,12 +101,13 @@ class CarController():
stopping = (actuators.longControlState == LongCtrlState.stopping) stopping = (actuators.longControlState == LongCtrlState.stopping)
set_speed_in_units = hud_speed * (CV.MS_TO_MPH if CS.clu11["CF_Clu_SPEED_UNIT"] == 1 else CV.MS_TO_KPH) set_speed_in_units = hud_speed * (CV.MS_TO_MPH if CS.clu11["CF_Clu_SPEED_UNIT"] == 1 else CV.MS_TO_KPH)
can_sends.extend(create_acc_commands(self.packer, enabled, accel, jerk, int(frame / 2), lead_visible, set_speed_in_units, stopping)) can_sends.extend(create_acc_commands(self.packer, enabled, accel, jerk, int(frame / 2), lead_visible, set_speed_in_units, stopping))
self.accel = accel
# 20 Hz LFA MFA message # 20 Hz LFA MFA message
if frame % 5 == 0 and self.car_fingerprint in [CAR.SONATA, CAR.PALISADE, CAR.IONIQ, CAR.KIA_NIRO_EV, CAR.KIA_NIRO_HEV_2021, if frame % 5 == 0 and self.car_fingerprint in [CAR.SONATA, CAR.PALISADE, CAR.IONIQ, CAR.KIA_NIRO_EV, CAR.KIA_NIRO_HEV_2021,
CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV, CAR.KIA_CEED, CAR.KIA_SELTOS, CAR.KONA_EV, CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV, CAR.KIA_CEED, CAR.KIA_SELTOS, CAR.KONA_EV,
CAR.ELANTRA_2021, CAR.ELANTRA_HEV_2021, CAR.SONATA_HYBRID, CAR.KONA_HEV, CAR.SANTA_FE_2022, CAR.ELANTRA_2021, CAR.ELANTRA_HEV_2021, CAR.SONATA_HYBRID, CAR.KONA_HEV, CAR.SANTA_FE_2022,
CAR.KIA_K5_2021, CAR.IONIQ_HEV_2022, CAR.SANTA_FE_HEV_2022, CAR.GENESIS_G70_2020]: CAR.KIA_K5_2021, CAR.IONIQ_HEV_2022, CAR.SANTA_FE_HEV_2022, CAR.GENESIS_G70_2020, CAR.SANTA_FE_PHEV_2022]:
can_sends.append(create_lfahda_mfc(self.packer, enabled)) can_sends.append(create_lfahda_mfc(self.packer, enabled))
# 5 Hz ACC options # 5 Hz ACC options
@ -116,4 +118,8 @@ class CarController():
if frame % 50 == 0 and CS.CP.openpilotLongitudinalControl: if frame % 50 == 0 and CS.CP.openpilotLongitudinalControl:
can_sends.append(create_frt_radar_opt(self.packer)) can_sends.append(create_frt_radar_opt(self.packer))
return can_sends new_actuators = actuators.copy()
new_actuators.steer = apply_steer / self.p.STEER_MAX
new_actuators.accel = self.accel
return new_actuators, can_sends

@ -19,7 +19,7 @@ def create_lkas11(packer, frame, car_fingerprint, apply_steer, steer_req,
if car_fingerprint in [CAR.SONATA, CAR.PALISADE, CAR.KIA_NIRO_EV, CAR.KIA_NIRO_HEV_2021, CAR.SANTA_FE, if car_fingerprint in [CAR.SONATA, CAR.PALISADE, CAR.KIA_NIRO_EV, CAR.KIA_NIRO_HEV_2021, CAR.SANTA_FE,
CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV, CAR.KIA_SELTOS, CAR.ELANTRA_2021, CAR.GENESIS_G70_2020, CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV, CAR.KIA_SELTOS, CAR.ELANTRA_2021, CAR.GENESIS_G70_2020,
CAR.ELANTRA_HEV_2021, CAR.SONATA_HYBRID, CAR.KONA_EV, CAR.KONA_HEV, CAR.SANTA_FE_2022, CAR.ELANTRA_HEV_2021, CAR.SONATA_HYBRID, CAR.KONA_EV, CAR.KONA_HEV, CAR.SANTA_FE_2022,
CAR.KIA_K5_2021, CAR.IONIQ_HEV_2022, CAR.SANTA_FE_HEV_2022]: CAR.KIA_K5_2021, CAR.IONIQ_HEV_2022, CAR.SANTA_FE_HEV_2022, CAR.SANTA_FE_PHEV_2022]:
values["CF_Lkas_LdwsActivemode"] = int(left_lane) + (int(right_lane) << 1) values["CF_Lkas_LdwsActivemode"] = int(left_lane) + (int(right_lane) << 1)
values["CF_Lkas_LdwsOpt_USM"] = 2 values["CF_Lkas_LdwsOpt_USM"] = 2

@ -45,7 +45,7 @@ class CarInterface(CarInterfaceBase):
ret.longitudinalActuatorDelayUpperBound = 1.0 # s ret.longitudinalActuatorDelayUpperBound = 1.0 # s
if candidate in [CAR.SANTA_FE, CAR.SANTA_FE_2022, CAR.SANTA_FE_HEV_2022]: if candidate in [CAR.SANTA_FE, CAR.SANTA_FE_2022, CAR.SANTA_FE_HEV_2022, CAR.SANTA_FE_PHEV_2022]:
ret.lateralTuning.pid.kf = 0.00005 ret.lateralTuning.pid.kf = 0.00005
ret.mass = 3982. * CV.LB_TO_KG + STD_CARGO_KG ret.mass = 3982. * CV.LB_TO_KG + STD_CARGO_KG
ret.wheelbase = 2.766 ret.wheelbase = 2.766
@ -347,8 +347,9 @@ class CarInterface(CarInterfaceBase):
return self.CS.out return self.CS.out
def apply(self, c): def apply(self, c):
can_sends = self.CC.update(c.enabled, self.CS, self.frame, c.actuators, hud_control = c.hudControl
c.cruiseControl.cancel, c.hudControl.visualAlert, c.hudControl.setSpeed, c.hudControl.leftLaneVisible, ret = self.CC.update(c.enabled, self.CS, self.frame, c.actuators,
c.hudControl.rightLaneVisible, c.hudControl.leftLaneDepart, c.hudControl.rightLaneDepart) c.cruiseControl.cancel, hud_control.visualAlert, hud_control.setSpeed, hud_control.leftLaneVisible,
hud_control.rightLaneVisible, hud_control.leftLaneDepart, hud_control.rightLaneDepart)
self.frame += 1 self.frame += 1
return can_sends return ret

@ -10,13 +10,14 @@ class CarControllerParams:
ACCEL_MAX = 2.0 # m/s ACCEL_MAX = 2.0 # m/s
def __init__(self, CP): def __init__(self, CP):
if CP.carFingerprint in [CAR.SONATA, CAR.PALISADE, CAR.SANTA_FE, CAR.VELOSTER, CAR.GENESIS_G70, CAR.GENESIS_G70_2020, # To determine the limit for your car, find the maximum value that the stock LKAS will request.
CAR.IONIQ_EV_2020, CAR.KIA_CEED, CAR.KIA_SELTOS, CAR.ELANTRA_2021, # If the max stock LKAS request is <384, add your car to this list.
CAR.ELANTRA_HEV_2021, CAR.SONATA_HYBRID, CAR.KONA_HEV, CAR.SANTA_FE_2022, if CP.carFingerprint in [CAR.GENESIS_G80, CAR.GENESIS_G90, CAR.ELANTRA, CAR.HYUNDAI_GENESIS, CAR.ELANTRA_GT_I30, CAR.IONIQ,
CAR.KIA_K5_2021, CAR.KONA_EV, CAR.KONA, CAR.IONIQ_HEV_2022, CAR.SANTA_FE_HEV_2022]: CAR.IONIQ_EV_LTD, CAR.IONIQ_PHEV, CAR.SANTA_FE_PHEV_2022, CAR.SONATA_LF, CAR.KIA_FORTE, CAR.KIA_NIRO_EV,
self.STEER_MAX = 384 CAR.KIA_NIRO_HEV, CAR.KIA_NIRO_HEV_2021, CAR.KIA_OPTIMA_H, CAR.KIA_OPTIMA, CAR.KIA_SORENTO, CAR.KIA_STINGER]:
else:
self.STEER_MAX = 255 self.STEER_MAX = 255
else:
self.STEER_MAX = 384
self.STEER_DELTA_UP = 3 self.STEER_DELTA_UP = 3
self.STEER_DELTA_DOWN = 7 self.STEER_DELTA_DOWN = 7
self.STEER_DRIVER_ALLOWANCE = 50 self.STEER_DRIVER_ALLOWANCE = 50
@ -41,6 +42,7 @@ class CAR:
SANTA_FE = "HYUNDAI SANTA FE 2019" SANTA_FE = "HYUNDAI SANTA FE 2019"
SANTA_FE_2022 = "HYUNDAI SANTA FE 2022" SANTA_FE_2022 = "HYUNDAI SANTA FE 2022"
SANTA_FE_HEV_2022 = "HYUNDAI SANTA FE HYBRID 2022" SANTA_FE_HEV_2022 = "HYUNDAI SANTA FE HYBRID 2022"
SANTA_FE_PHEV_2022 = "HYUNDAI SANTA FE PlUG-IN HYBRID 2022"
SONATA = "HYUNDAI SONATA 2020" SONATA = "HYUNDAI SONATA 2020"
SONATA_LF = "HYUNDAI SONATA 2019" SONATA_LF = "HYUNDAI SONATA 2019"
PALISADE = "HYUNDAI PALISADE 2020" PALISADE = "HYUNDAI PALISADE 2020"
@ -498,6 +500,23 @@ FW_VERSIONS = {
b'\xf1\x87391312MTC1', b'\xf1\x87391312MTC1',
], ],
}, },
CAR.SANTA_FE_PHEV_2022: {
(Ecu.fwdRadar, 0x7d0, None): [
b'\xf1\x8799110CL500\xf1\x00TMhe SCC FHCUP 1.00 1.00 99110-CL500 ',
],
(Ecu.eps, 0x7d4, None): [
b'\xf1\x00TM MDPS C 1.00 1.02 56310-CLAC0 4TSHC102',
],
(Ecu.fwdCamera, 0x7c4, None): [
b'\xf1\x00TMP MFC AT USA LHD 1.00 1.03 99211-S1500 210224',
],
(Ecu.transmission, 0x7e1, None): [
b'\xf1\x8795441-3D121\x00\xf1\x81E16\x00\x00\x00\x00\x00\x00\x00\xf1\x00PSBG2333 E16\x00\x00\x00\x00\x00\x00\x00TTM2P16SA0o\x88^\xbe',
],
(Ecu.engine, 0x7e0, None): [
b'\xf1\x87391312MTF0',
],
},
CAR.KIA_STINGER: { CAR.KIA_STINGER: {
(Ecu.fwdRadar, 0x7d0, None): [ (Ecu.fwdRadar, 0x7d0, None): [
b'\xf1\x00CK__ SCC F_CUP 1.00 1.01 96400-J5100 ', b'\xf1\x00CK__ SCC F_CUP 1.00 1.01 96400-J5100 ',
@ -777,25 +796,22 @@ FW_VERSIONS = {
b'\xf1\x00DEev SCC F-CUP 1.00 1.00 99110-Q4000 ', b'\xf1\x00DEev SCC F-CUP 1.00 1.00 99110-Q4000 ',
b'\xf1\x00DEev SCC F-CUP 1.00 1.02 96400-Q4100 ', b'\xf1\x00DEev SCC F-CUP 1.00 1.02 96400-Q4100 ',
b'\xf1\x00DEev SCC F-CUP 1.00 1.03 96400-Q4100 ', b'\xf1\x00DEev SCC F-CUP 1.00 1.03 96400-Q4100 ',
b'\xf1\x00OSev SCC F-CUP 1.00 1.01 99110-K4000 ',
b'\xf1\x8799110Q4000\xf1\x00DEev SCC F-CUP 1.00 1.00 99110-Q4000 ', 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\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\000DEev SCC F-CUP 1.00 1.00 99110-Q4500 ',
], b'\xf1\x8799110Q4600\xf1\x00DEev SCC FNCUP 1.00 1.00 99110-Q4600 ',
(Ecu.esp, 0x7D1, None): [ b'\xf1\x8799110Q4600\xf1\x00DEev SCC FHCUP 1.00 1.00 99110-Q4600 ',
b'\xf1\x00OS IEB \r 212 \x11\x13 58520-K4000',
], ],
(Ecu.eps, 0x7D4, None): [ (Ecu.eps, 0x7D4, None): [
b'\xf1\x00DE MDPS C 1.00 1.05 56310Q4000\x00 4DEEC105', b'\xf1\x00DE MDPS C 1.00 1.05 56310Q4000\x00 4DEEC105',
b'\xf1\x00DE MDPS C 1.00 1.05 56310Q4100\x00 4DEEC105', b'\xf1\x00DE MDPS C 1.00 1.05 56310Q4100\x00 4DEEC105',
b'\xf1\x00OS MDPS C 1.00 1.04 56310K4050\x00 4OEDC104',
], ],
(Ecu.fwdCamera, 0x7C4, None): [ (Ecu.fwdCamera, 0x7C4, None): [
b'\xf1\000DEE MFC AT EUR LHD 1.00 1.00 99211-Q4100 200706', 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-Q4000 191211', 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.00 99211-Q4000 191211',
b'\xf1\x00DEE MFC AT USA LHD 1.00 1.03 95740-Q4000 180821', b'\xf1\x00DEE MFC AT USA LHD 1.00 1.03 95740-Q4000 180821',
b'\xf1\x00OSE LKAS AT EUR LHD 1.00 1.00 95740-K4100 W40', b'\xf1\x00DEE MFC AT USA LHD 1.00 1.01 99211-Q4500 210428',
], ],
}, },
CAR.KIA_NIRO_HEV: { CAR.KIA_NIRO_HEV: {
@ -981,25 +997,25 @@ FW_VERSIONS = {
} }
CHECKSUM = { CHECKSUM = {
"crc8": [CAR.SANTA_FE, CAR.SONATA, CAR.PALISADE, CAR.KIA_SELTOS, CAR.ELANTRA_2021, CAR.ELANTRA_HEV_2021, CAR.SONATA_HYBRID, CAR.SANTA_FE_2022, CAR.KIA_K5_2021, CAR.SANTA_FE_HEV_2022], "crc8": [CAR.SANTA_FE, CAR.SONATA, CAR.PALISADE, CAR.KIA_SELTOS, CAR.ELANTRA_2021, CAR.ELANTRA_HEV_2021, CAR.SONATA_HYBRID, CAR.SANTA_FE_2022, CAR.KIA_K5_2021, CAR.SANTA_FE_HEV_2022, CAR.SANTA_FE_PHEV_2022],
"6B": [CAR.KIA_SORENTO, CAR.HYUNDAI_GENESIS], "6B": [CAR.KIA_SORENTO, CAR.HYUNDAI_GENESIS],
} }
FEATURES = { FEATURES = {
# which message has the gear # which message has the gear
"use_cluster_gears": set([CAR.ELANTRA, CAR.ELANTRA_GT_I30, CAR.KONA]), "use_cluster_gears": {CAR.ELANTRA, CAR.ELANTRA_GT_I30, CAR.KONA},
"use_tcu_gears": set([CAR.KIA_OPTIMA, CAR.SONATA_LF, CAR.VELOSTER]), "use_tcu_gears": {CAR.KIA_OPTIMA, CAR.SONATA_LF, CAR.VELOSTER},
"use_elect_gears": set([CAR.KIA_NIRO_EV, CAR.KIA_NIRO_HEV, CAR.KIA_NIRO_HEV_2021, CAR.KIA_OPTIMA_H, CAR.IONIQ_EV_LTD, CAR.KONA_EV, CAR.IONIQ, CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV, CAR.ELANTRA_HEV_2021,CAR.SONATA_HYBRID, CAR.KONA_HEV, CAR.IONIQ_HEV_2022, CAR.SANTA_FE_HEV_2022]), "use_elect_gears": {CAR.KIA_NIRO_EV, CAR.KIA_NIRO_HEV, CAR.KIA_NIRO_HEV_2021, CAR.KIA_OPTIMA_H, CAR.IONIQ_EV_LTD, CAR.KONA_EV, CAR.IONIQ, CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV, CAR.ELANTRA_HEV_2021,CAR.SONATA_HYBRID, CAR.KONA_HEV, CAR.IONIQ_HEV_2022, CAR.SANTA_FE_HEV_2022, CAR.SANTA_FE_PHEV_2022},
# these cars use the FCA11 message for the AEB and FCW signals, all others use SCC12 # these cars use the FCA11 message for the AEB and FCW signals, all others use SCC12
"use_fca": set([CAR.SONATA, CAR.SONATA_HYBRID, CAR.ELANTRA, CAR.ELANTRA_2021, CAR.ELANTRA_HEV_2021, CAR.ELANTRA_GT_I30, CAR.KIA_STINGER, CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV, CAR.KONA_EV, CAR.KIA_FORTE, CAR.KIA_NIRO_EV, CAR.PALISADE, CAR.GENESIS_G70, CAR.GENESIS_G70_2020, CAR.KONA, CAR.SANTA_FE, CAR.KIA_SELTOS, CAR.KONA_HEV, CAR.SANTA_FE_2022, CAR.KIA_K5_2021, CAR.IONIQ_HEV_2022, CAR.SANTA_FE_HEV_2022]), "use_fca": {CAR.SONATA, CAR.SONATA_HYBRID, CAR.ELANTRA, CAR.ELANTRA_2021, CAR.ELANTRA_HEV_2021, CAR.ELANTRA_GT_I30, CAR.KIA_STINGER, CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV, CAR.KONA_EV, CAR.KIA_FORTE, CAR.KIA_NIRO_EV, CAR.PALISADE, CAR.GENESIS_G70, CAR.GENESIS_G70_2020, CAR.KONA, CAR.SANTA_FE, CAR.KIA_SELTOS, CAR.KONA_HEV, CAR.SANTA_FE_2022, CAR.KIA_K5_2021, CAR.IONIQ_HEV_2022, CAR.SANTA_FE_HEV_2022, CAR.SANTA_FE_PHEV_2022},
} }
HYBRID_CAR = set([CAR.IONIQ_PHEV, CAR.ELANTRA_HEV_2021, CAR.KIA_NIRO_HEV, CAR.KIA_NIRO_HEV_2021, CAR.SONATA_HYBRID, CAR.KONA_HEV, CAR.IONIQ, CAR.IONIQ_HEV_2022, CAR.SANTA_FE_HEV_2022]) # these cars use a different gas signal HYBRID_CAR = {CAR.IONIQ_PHEV, CAR.ELANTRA_HEV_2021, CAR.KIA_NIRO_HEV, CAR.KIA_NIRO_HEV_2021, CAR.SONATA_HYBRID, CAR.KONA_HEV, CAR.IONIQ, CAR.IONIQ_HEV_2022, CAR.SANTA_FE_HEV_2022, CAR.SANTA_FE_PHEV_2022} # these cars use a different gas signal
EV_CAR = set([CAR.IONIQ_EV_2020, CAR.IONIQ_EV_LTD, CAR.KONA_EV, CAR.KIA_NIRO_EV]) EV_CAR = {CAR.IONIQ_EV_2020, CAR.IONIQ_EV_LTD, CAR.KONA_EV, CAR.KIA_NIRO_EV}
# these cars require a special panda safety mode due to missing counters and checksums in the messages # these cars require a special panda safety mode due to missing counters and checksums in the messages
LEGACY_SAFETY_MODE_CAR = set([CAR.HYUNDAI_GENESIS, CAR.IONIQ_EV_2020, CAR.IONIQ_EV_LTD, CAR.IONIQ_PHEV, CAR.IONIQ, CAR.KONA_EV, CAR.KIA_SORENTO, CAR.SONATA_LF, CAR.KIA_NIRO_EV, CAR.KIA_OPTIMA, CAR.VELOSTER, CAR.KIA_STINGER, CAR.GENESIS_G70, CAR.GENESIS_G80, CAR.KIA_CEED, CAR.ELANTRA, CAR.IONIQ_HEV_2022]) LEGACY_SAFETY_MODE_CAR = {CAR.HYUNDAI_GENESIS, CAR.IONIQ_EV_2020, CAR.IONIQ_EV_LTD, CAR.IONIQ_PHEV, CAR.IONIQ, CAR.KONA_EV, CAR.KIA_SORENTO, CAR.SONATA_LF, CAR.KIA_NIRO_EV, CAR.KIA_OPTIMA, CAR.VELOSTER, CAR.KIA_STINGER, CAR.GENESIS_G70, CAR.GENESIS_G80, CAR.KIA_CEED, CAR.ELANTRA, CAR.IONIQ_HEV_2022}
# If 0x500 is present on bus 1 it probably has a Mando radar outputting radar points. # If 0x500 is present on bus 1 it probably has a Mando radar outputting radar points.
# If no points are outputted by default it might be possible to turn it on using selfdrive/debug/hyundai_enable_radar_points.py # If no points are outputted by default it might be possible to turn it on using selfdrive/debug/hyundai_enable_radar_points.py
@ -1034,6 +1050,7 @@ DBC = {
CAR.SANTA_FE: dbc_dict('hyundai_kia_generic', 'hyundai_kia_mando_front_radar'), CAR.SANTA_FE: dbc_dict('hyundai_kia_generic', 'hyundai_kia_mando_front_radar'),
CAR.SANTA_FE_2022: dbc_dict('hyundai_kia_generic', None), CAR.SANTA_FE_2022: dbc_dict('hyundai_kia_generic', None),
CAR.SANTA_FE_HEV_2022: dbc_dict('hyundai_kia_generic', None), CAR.SANTA_FE_HEV_2022: dbc_dict('hyundai_kia_generic', None),
CAR.SANTA_FE_PHEV_2022: dbc_dict('hyundai_kia_generic', None),
CAR.SONATA: dbc_dict('hyundai_kia_generic', 'hyundai_kia_mando_front_radar'), CAR.SONATA: dbc_dict('hyundai_kia_generic', 'hyundai_kia_mando_front_radar'),
CAR.SONATA_LF: dbc_dict('hyundai_kia_generic', None), # Has 0x5XX messages, but different format CAR.SONATA_LF: dbc_dict('hyundai_kia_generic', None), # Has 0x5XX messages, but different format
CAR.PALISADE: dbc_dict('hyundai_kia_generic', 'hyundai_kia_mando_front_radar'), CAR.PALISADE: dbc_dict('hyundai_kia_generic', 'hyundai_kia_mando_front_radar'),

@ -1,6 +1,7 @@
import os import os
import time import time
from typing import Dict from abc import abstractmethod, ABC
from typing import Dict, Tuple, List
from cereal import car from cereal import car
from common.kalman.simple_kalman import KF1D from common.kalman.simple_kalman import KF1D
@ -22,7 +23,7 @@ ACCEL_MIN = -3.5
# generic car and radar interfaces # generic car and radar interfaces
class CarInterfaceBase(): class CarInterfaceBase(ABC):
def __init__(self, CP, CarController, CarState): def __init__(self, CP, CarController, CarState):
self.CP = CP self.CP = CP
self.VM = VehicleModel(CP) self.VM = VehicleModel(CP)
@ -48,8 +49,9 @@ class CarInterfaceBase():
return ACCEL_MIN, ACCEL_MAX return ACCEL_MIN, ACCEL_MAX
@staticmethod @staticmethod
@abstractmethod
def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None): def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None):
raise NotImplementedError pass
@staticmethod @staticmethod
def init(CP, logcan, sendcan): def init(CP, logcan, sendcan):
@ -82,13 +84,12 @@ class CarInterfaceBase():
ret.minEnableSpeed = -1. # enable is done by stock ACC, so ignore this ret.minEnableSpeed = -1. # enable is done by stock ACC, so ignore this
ret.steerRatioRear = 0. # no rear steering, at least on the listed cars aboveA ret.steerRatioRear = 0. # no rear steering, at least on the listed cars aboveA
ret.openpilotLongitudinalControl = False ret.openpilotLongitudinalControl = False
ret.minSpeedCan = 0.3
ret.startAccel = -0.8 ret.startAccel = -0.8
ret.stopAccel = -2.0 ret.stopAccel = -2.0
ret.startingAccelRate = 3.2 # brake_travel/s while releasing on restart ret.startingAccelRate = 3.2 # brake_travel/s while releasing on restart
ret.stoppingDecelRate = 0.8 # brake_travel/s while trying to stop ret.stoppingDecelRate = 0.8 # brake_travel/s while trying to stop
ret.vEgoStopping = 0.5 ret.vEgoStopping = 0.5
ret.vEgoStarting = 0.5 ret.vEgoStarting = 0.5 # needs to be >= vEgoStopping to avoid state transition oscillation
ret.stoppingControl = True ret.stoppingControl = True
ret.longitudinalTuning.deadzoneBP = [0.] ret.longitudinalTuning.deadzoneBP = [0.]
ret.longitudinalTuning.deadzoneV = [0.] ret.longitudinalTuning.deadzoneV = [0.]
@ -100,13 +101,13 @@ class CarInterfaceBase():
ret.longitudinalActuatorDelayUpperBound = 0.15 ret.longitudinalActuatorDelayUpperBound = 0.15
return ret return ret
# returns a car.CarState, pass in car.CarControl @abstractmethod
def update(self, c, can_strings): def update(self, c: car.CarControl, can_strings: List[bytes]) -> car.CarState:
raise NotImplementedError pass
# return sendcan, pass in a car.CarControl @abstractmethod
def apply(self, c): def apply(self, c: car.CarControl) -> Tuple[car.CarControl.Actuators, List[bytes]]:
raise NotImplementedError 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, gas_resume_speed=-1, pcm_enable=True):
events = Events() events = Events()
@ -169,7 +170,7 @@ class CarInterfaceBase():
return events return events
class RadarInterfaceBase(): class RadarInterfaceBase(ABC):
def __init__(self, CP): def __init__(self, CP):
self.pts = {} self.pts = {}
self.delay = 0 self.delay = 0
@ -183,7 +184,7 @@ class RadarInterfaceBase():
return ret return ret
class CarStateBase: class CarStateBase(ABC):
def __init__(self, CP): def __init__(self, CP):
self.CP = CP self.CP = CP
self.car_fingerprint = CP.carFingerprint self.car_fingerprint = CP.carFingerprint

@ -58,4 +58,8 @@ class CarController():
# send steering command # send steering command
can_sends.append(mazdacan.create_steering_control(self.packer, CS.CP.carFingerprint, can_sends.append(mazdacan.create_steering_control(self.packer, CS.CP.carFingerprint,
frame, apply_steer, CS.cam_lkas)) frame, apply_steer, CS.cam_lkas))
return can_sends
new_actuators = c.actuators.copy()
new_actuators.steer = apply_steer / CarControllerParams.STEER_MAX
return new_actuators, can_sends

@ -95,6 +95,6 @@ class CarInterface(CarInterfaceBase):
return self.CS.out return self.CS.out
def apply(self, c): def apply(self, c):
can_sends = self.CC.update(c, self.CS, self.frame) ret = self.CC.update(c, self.CS, self.frame)
self.frame += 1 self.frame += 1
return can_sends return ret

@ -50,9 +50,11 @@ FW_VERSIONS = {
b'PYFC-188K2-J\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'PYFC-188K2-J\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PYFD-188K2-J\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'PYFD-188K2-J\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PYNF-188K2-F\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'PYNF-188K2-F\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PX2F-188K2-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PX2G-188K2-D\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'PX2G-188K2-D\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PX2H-188K2-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'PX2H-188K2-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PX2H-188K2-D\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'PX2H-188K2-D\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PX2H-188K2-G\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PX2K-188K2-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'PX2K-188K2-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PX38-188K2-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'PX38-188K2-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PX42-188K2-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'PX42-188K2-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
@ -65,6 +67,7 @@ FW_VERSIONS = {
b'K131-67XK2-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'K131-67XK2-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'K131-67XK2-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'K131-67XK2-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'K131-67XK2-E\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'K131-67XK2-E\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'K131-67XK2-F\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
], ],
(Ecu.esp, 0x760, None): [ (Ecu.esp, 0x760, None): [
b'K123-437K2-E\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'K123-437K2-E\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
@ -78,7 +81,9 @@ FW_VERSIONS = {
b'B61L-67XK2-T\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'B61L-67XK2-T\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'B61L-67XK2-V\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'B61L-67XK2-V\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'GSH7-67XK2-J\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'GSH7-67XK2-J\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'GSH7-67XK2-M\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'GSH7-67XK2-N\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'GSH7-67XK2-N\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'GSH7-67XK2-R\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
], ],
(Ecu.transmission, 0x7e1, None): [ (Ecu.transmission, 0x7e1, None): [
b'PA66-21PS1-A\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'PA66-21PS1-A\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
@ -87,10 +92,12 @@ FW_VERSIONS = {
b'PX68-21PS1-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'PX68-21PS1-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PYB1-21PS1-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'PYB1-21PS1-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PYB1-21PS1-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'PYB1-21PS1-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PYB1-21PS1-G\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PYB2-21PS1-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'PYB2-21PS1-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PYB2-21PS1-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'PYB2-21PS1-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PYB2-21PS1-D\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'PYB2-21PS1-D\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PYB2-21PS1-G\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'PYB2-21PS1-G\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PYB2-21PS1-H\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PYNC-21PS1-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'PYNC-21PS1-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'SH9T-21PS1-D\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'SH9T-21PS1-D\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
], ],
@ -145,10 +152,12 @@ FW_VERSIONS = {
b'BHN1-3210X-J-00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'BHN1-3210X-J-00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'K070-3210X-C-00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'K070-3210X-C-00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'KR11-3210X-K-00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'KR11-3210X-K-00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
], ],
(Ecu.engine, 0x7e0, None): [ (Ecu.engine, 0x7e0, None): [
b'P5JD-188K2-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'P5JD-188K2-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PY2P-188K2-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'PY2P-188K2-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PYJW-188K2-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PYKC-188K2-D\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'PYKC-188K2-D\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PYKE-188K2-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'PYKE-188K2-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
], ],
@ -163,11 +172,13 @@ FW_VERSIONS = {
(Ecu.fwdCamera, 0x706, None): [ (Ecu.fwdCamera, 0x706, None): [
b'B61L-67XK2-D\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'B61L-67XK2-D\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'B61L-67XK2-P\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'B61L-67XK2-P\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'B61L-67XK2-Q\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'B61L-67XK2-T\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'B61L-67XK2-T\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
], ],
(Ecu.transmission, 0x7e1, None): [ (Ecu.transmission, 0x7e1, None): [
b'PY2S-21PS1-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'PY2S-21PS1-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'P52G-21PS1-F\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'P52G-21PS1-F\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PYKA-21PS1-A\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PYKE-21PS1-A\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'PYKE-21PS1-A\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PYKE-21PS1-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'PYKE-21PS1-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
], ],
@ -236,7 +247,7 @@ DBC = {
} }
# Gen 1 hardware: same CAN messages and same camera # Gen 1 hardware: same CAN messages and same camera
GEN1 = set([CAR.CX5, CAR.CX9, CAR.CX9_2021, CAR.MAZDA3, CAR.MAZDA6]) GEN1 = {CAR.CX5, CAR.CX9, CAR.CX9_2021, CAR.MAZDA3, CAR.MAZDA6}
# Cars with a steering lockout # Cars with a steering lockout
STEER_LOCKOUT_CAR = set([CAR.CX5, CAR.CX9, CAR.MAZDA3, CAR.MAZDA6]) STEER_LOCKOUT_CAR = {CAR.CX5, CAR.CX9, CAR.MAZDA3, CAR.MAZDA6}

@ -88,4 +88,5 @@ class CarInterface(CarInterfaceBase):
def apply(self, c): def apply(self, c):
# in mock no carcontrols # in mock no carcontrols
return [] actuators = car.CarControl.Actuators.new_message()
return actuators, []

@ -87,4 +87,7 @@ class CarController():
self.packer, lkas_hud_info_msg, steer_hud_alert self.packer, lkas_hud_info_msg, steer_hud_alert
)) ))
return can_sends new_actuators = actuators.copy()
new_actuators.steeringAngleDeg = apply_angle
return new_actuators, can_sends

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

@ -8,11 +8,11 @@ class CarController():
def __init__(self, dbc_name, CP, VM): def __init__(self, dbc_name, CP, VM):
self.apply_steer_last = 0 self.apply_steer_last = 0
self.es_distance_cnt = -1 self.es_distance_cnt = -1
self.es_accel_cnt = -1
self.es_lkas_cnt = -1 self.es_lkas_cnt = -1
self.cruise_button_prev = 0 self.cruise_button_prev = 0
self.steer_rate_limited = False self.steer_rate_limited = False
self.p = CarControllerParams(CP)
self.packer = CANPacker(DBC[CP.carFingerprint]['pt']) 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, enabled, CS, frame, actuators, pcm_cancel_cmd, visual_alert, left_line, right_line, left_lane_depart, right_lane_depart):
@ -20,23 +20,23 @@ class CarController():
can_sends = [] can_sends = []
# *** steering *** # *** steering ***
if (frame % CarControllerParams.STEER_STEP) == 0: if (frame % self.p.STEER_STEP) == 0:
apply_steer = int(round(actuators.steer * CarControllerParams.STEER_MAX)) apply_steer = int(round(actuators.steer * self.p.STEER_MAX))
# limits due to driver torque # limits due to driver torque
new_steer = int(round(apply_steer)) new_steer = int(round(apply_steer))
apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, CarControllerParams) 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 self.steer_rate_limited = new_steer != apply_steer
if not enabled: if not enabled:
apply_steer = 0 apply_steer = 0
if CS.CP.carFingerprint in PREGLOBAL_CARS: if CS.CP.carFingerprint in PREGLOBAL_CARS:
can_sends.append(subarucan.create_preglobal_steering_control(self.packer, apply_steer, frame, CarControllerParams.STEER_STEP)) can_sends.append(subarucan.create_preglobal_steering_control(self.packer, apply_steer, frame, self.p.STEER_STEP))
else: else:
can_sends.append(subarucan.create_steering_control(self.packer, apply_steer, frame, CarControllerParams.STEER_STEP)) can_sends.append(subarucan.create_steering_control(self.packer, apply_steer, frame, self.p.STEER_STEP))
self.apply_steer_last = apply_steer self.apply_steer_last = apply_steer
@ -44,7 +44,7 @@ class CarController():
# *** alerts and pcm cancel *** # *** alerts and pcm cancel ***
if CS.CP.carFingerprint in PREGLOBAL_CARS: if CS.CP.carFingerprint in PREGLOBAL_CARS:
if self.es_accel_cnt != CS.es_accel_msg["Counter"]: if self.es_distance_cnt != CS.es_distance_msg["Counter"]:
# 1 = main, 2 = set shallow, 3 = set deep, 4 = resume shallow, 5 = resume deep # 1 = main, 2 = set shallow, 3 = set deep, 4 = resume shallow, 5 = resume deep
# disengage ACC when OP is disengaged # disengage ACC when OP is disengaged
if pcm_cancel_cmd: if pcm_cancel_cmd:
@ -60,8 +60,8 @@ class CarController():
cruise_button = 0 cruise_button = 0
self.cruise_button_prev = cruise_button self.cruise_button_prev = cruise_button
can_sends.append(subarucan.create_es_throttle_control(self.packer, cruise_button, CS.es_accel_msg)) can_sends.append(subarucan.create_preglobal_es_distance(self.packer, cruise_button, CS.es_distance_msg))
self.es_accel_cnt = CS.es_accel_msg["Counter"] self.es_distance_cnt = CS.es_distance_msg["Counter"]
else: else:
if self.es_distance_cnt != CS.es_distance_msg["Counter"]: if self.es_distance_cnt != CS.es_distance_msg["Counter"]:
@ -72,4 +72,7 @@ class CarController():
can_sends.append(subarucan.create_es_lkas(self.packer, CS.es_lkas_msg, enabled, visual_alert, left_line, right_line, left_lane_depart, right_lane_depart)) can_sends.append(subarucan.create_es_lkas(self.packer, CS.es_lkas_msg, enabled, visual_alert, left_line, right_line, left_lane_depart, right_lane_depart))
self.es_lkas_cnt = CS.es_lkas_msg["Counter"] self.es_lkas_cnt = CS.es_lkas_msg["Counter"]
return can_sends new_actuators = actuators.copy()
new_actuators.steer = self.apply_steer_last / self.p.STEER_MAX
return new_actuators, can_sends

@ -65,14 +65,13 @@ class CarState(CarStateBase):
ret.steerError = cp.vl["Steering_Torque"]["Steer_Error_1"] == 1 ret.steerError = cp.vl["Steering_Torque"]["Steer_Error_1"] == 1
if self.car_fingerprint in PREGLOBAL_CARS: if self.car_fingerprint in PREGLOBAL_CARS:
self.cruise_button = cp_cam.vl["ES_CruiseThrottle"]["Cruise_Button"] self.cruise_button = cp_cam.vl["ES_Distance"]["Cruise_Button"]
self.ready = not cp_cam.vl["ES_DashStatus"]["Not_Ready_Startup"] self.ready = not cp_cam.vl["ES_DashStatus"]["Not_Ready_Startup"]
self.es_accel_msg = copy.copy(cp_cam.vl["ES_CruiseThrottle"])
else: else:
ret.steerWarning = cp.vl["Steering_Torque"]["Steer_Warning"] == 1 ret.steerWarning = cp.vl["Steering_Torque"]["Steer_Warning"] == 1
ret.cruiseState.nonAdaptive = cp_cam.vl["ES_DashStatus"]["Conventional_Cruise"] == 1 ret.cruiseState.nonAdaptive = cp_cam.vl["ES_DashStatus"]["Conventional_Cruise"] == 1
self.es_distance_msg = copy.copy(cp_cam.vl["ES_Distance"])
self.es_lkas_msg = copy.copy(cp_cam.vl["ES_LKAS_State"]) self.es_lkas_msg = copy.copy(cp_cam.vl["ES_LKAS_State"])
self.es_distance_msg = copy.copy(cp_cam.vl["ES_Distance"])
return ret return ret
@ -168,28 +167,28 @@ class CarState(CarStateBase):
("Cruise_Set_Speed", "ES_DashStatus", 0), ("Cruise_Set_Speed", "ES_DashStatus", 0),
("Not_Ready_Startup", "ES_DashStatus", 0), ("Not_Ready_Startup", "ES_DashStatus", 0),
("Throttle_Cruise", "ES_CruiseThrottle", 0), ("Cruise_Throttle", "ES_Distance", 0),
("Signal1", "ES_CruiseThrottle", 0), ("Signal1", "ES_Distance", 0),
("Cruise_Activated", "ES_CruiseThrottle", 0), ("Car_Follow", "ES_Distance", 0),
("Signal2", "ES_CruiseThrottle", 0), ("Signal2", "ES_Distance", 0),
("Brake_On", "ES_CruiseThrottle", 0), ("Brake_On", "ES_Distance", 0),
("Distance_Swap", "ES_CruiseThrottle", 0), ("Distance_Swap", "ES_Distance", 0),
("Standstill", "ES_CruiseThrottle", 0), ("Standstill", "ES_Distance", 0),
("Signal3", "ES_CruiseThrottle", 0), ("Signal3", "ES_Distance", 0),
("Close_Distance", "ES_CruiseThrottle", 0), ("Close_Distance", "ES_Distance", 0),
("Signal4", "ES_CruiseThrottle", 0), ("Signal4", "ES_Distance", 0),
("Standstill_2", "ES_CruiseThrottle", 0), ("Standstill_2", "ES_Distance", 0),
("Cruise_Fault", "ES_CruiseThrottle", 0), ("Cruise_Fault", "ES_Distance", 0),
("Signal5", "ES_CruiseThrottle", 0), ("Signal5", "ES_Distance", 0),
("Counter", "ES_CruiseThrottle", 0), ("Counter", "ES_Distance", 0),
("Signal6", "ES_CruiseThrottle", 0), ("Signal6", "ES_Distance", 0),
("Cruise_Button", "ES_CruiseThrottle", 0), ("Cruise_Button", "ES_Distance", 0),
("Signal7", "ES_CruiseThrottle", 0), ("Signal7", "ES_Distance", 0),
] ]
checks = [ checks = [
("ES_DashStatus", 20), ("ES_DashStatus", 20),
("ES_CruiseThrottle", 20), ("ES_Distance", 20),
] ]
else: else:
signals = [ signals = [

@ -45,6 +45,16 @@ class CarInterface(CarInterfaceBase):
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0., 20.], [0., 20.]] ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0., 20.], [0., 20.]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2, 0.3], [0.02, 0.03]] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2, 0.3], [0.02, 0.03]]
if candidate == CAR.IMPREZA_2020:
ret.mass = 1480. + STD_CARGO_KG
ret.wheelbase = 2.67
ret.centerToFront = ret.wheelbase * 0.5
ret.steerRatio = 17 # learned, 14 stock
ret.steerActuatorDelay = 0.1
ret.lateralTuning.pid.kf = 0.00005
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0., 14., 23.], [0., 14., 23.]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.045, 0.042, 0.20], [0.04, 0.035, 0.045]]
if candidate == CAR.FORESTER: if candidate == CAR.FORESTER:
ret.mass = 1568. + STD_CARGO_KG ret.mass = 1568. + STD_CARGO_KG
ret.wheelbase = 2.67 ret.wheelbase = 2.67
@ -112,8 +122,9 @@ class CarInterface(CarInterfaceBase):
return self.CS.out return self.CS.out
def apply(self, c): def apply(self, c):
can_sends = self.CC.update(c.enabled, self.CS, self.frame, c.actuators, hud_control = c.hudControl
c.cruiseControl.cancel, c.hudControl.visualAlert, ret = self.CC.update(c.enabled, self.CS, self.frame, c.actuators,
c.hudControl.leftLaneVisible, c.hudControl.rightLaneVisible, c.hudControl.leftLaneDepart, c.hudControl.rightLaneDepart) c.cruiseControl.cancel, hud_control.visualAlert,
hud_control.leftLaneVisible, hud_control.rightLaneVisible, hud_control.leftLaneDepart, hud_control.rightLaneDepart)
self.frame += 1 self.frame += 1
return can_sends return ret

@ -80,11 +80,11 @@ def create_preglobal_steering_control(packer, apply_steer, frame, steer_step):
return packer.make_can_msg("ES_LKAS", 0, values) return packer.make_can_msg("ES_LKAS", 0, values)
def create_es_throttle_control(packer, cruise_button, es_accel_msg): def create_preglobal_es_distance(packer, cruise_button, es_distance_msg):
values = copy.copy(es_accel_msg) values = copy.copy(es_distance_msg)
values["Cruise_Button"] = cruise_button values["Cruise_Button"] = cruise_button
values["Checksum"] = subaru_preglobal_checksum(packer, values, "ES_CruiseThrottle") values["Checksum"] = subaru_preglobal_checksum(packer, values, "ES_Distance")
return packer.make_can_msg("ES_CruiseThrottle", 0, values) return packer.make_can_msg("ES_Distance", 0, values)

@ -5,17 +5,22 @@ from cereal import car
Ecu = car.CarParams.Ecu Ecu = car.CarParams.Ecu
class CarControllerParams: class CarControllerParams:
STEER_MAX = 2047 # max_steer 4095 def __init__(self, CP):
STEER_STEP = 2 # how often we update the steer cmd if CP.carFingerprint == CAR.IMPREZA_2020:
STEER_DELTA_UP = 50 # torque increase per refresh, 0.8s to max self.STEER_MAX = 1439
STEER_DELTA_DOWN = 70 # torque decrease per refresh else:
STEER_DRIVER_ALLOWANCE = 60 # allowed driver torque before start limiting self.STEER_MAX = 2047
STEER_DRIVER_MULTIPLIER = 10 # weight driver torque heavily self.STEER_STEP = 2 # how often we update the steer cmd
STEER_DRIVER_FACTOR = 1 # from dbc self.STEER_DELTA_UP = 50 # torque increase per refresh, 0.8s to max
self.STEER_DELTA_DOWN = 70 # torque decrease per refresh
self.STEER_DRIVER_ALLOWANCE = 60 # allowed driver torque before start limiting
self.STEER_DRIVER_MULTIPLIER = 10 # weight driver torque heavily
self.STEER_DRIVER_FACTOR = 1 # from dbc
class CAR: class CAR:
ASCENT = "SUBARU ASCENT LIMITED 2019" ASCENT = "SUBARU ASCENT LIMITED 2019"
IMPREZA = "SUBARU IMPREZA LIMITED 2019" IMPREZA = "SUBARU IMPREZA LIMITED 2019"
IMPREZA_2020 = "SUBARU IMPREZA SPORT 2020"
FORESTER = "SUBARU FORESTER 2019" FORESTER = "SUBARU FORESTER 2019"
FORESTER_PREGLOBAL = "SUBARU FORESTER 2017 - 2018" FORESTER_PREGLOBAL = "SUBARU FORESTER 2017 - 2018"
LEGACY_PREGLOBAL = "SUBARU LEGACY 2015 - 2018" LEGACY_PREGLOBAL = "SUBARU LEGACY 2015 - 2018"
@ -30,6 +35,14 @@ FINGERPRINTS = {
CAR.IMPREZA: [{ CAR.IMPREZA: [{
2: 8, 64: 8, 65: 8, 72: 8, 73: 8, 280: 8, 281: 8, 290: 8, 312: 8, 313: 8, 314: 8, 315: 8, 316: 8, 326: 8, 372: 8, 544: 8, 545: 8, 546: 8, 552: 8, 554: 8, 557: 8, 576: 8, 577: 8, 722: 8, 801: 8, 802: 8, 805: 8, 808: 8, 811: 8, 816: 8, 826: 8, 827: 8, 837: 8, 838: 8, 839: 8, 842: 8, 912: 8, 915: 8, 940: 8, 1614: 8, 1617: 8, 1632: 8, 1650: 8, 1657: 8, 1658: 8, 1677: 8, 1697: 8, 1722: 8, 1743: 8, 1759: 8, 1786: 5, 1787: 5, 1788: 8, 1809: 8, 1813: 8, 1817: 8, 1821: 8, 1840: 8, 1848: 8, 1924: 8, 1932: 8, 1952: 8, 1960: 8 2: 8, 64: 8, 65: 8, 72: 8, 73: 8, 280: 8, 281: 8, 290: 8, 312: 8, 313: 8, 314: 8, 315: 8, 316: 8, 326: 8, 372: 8, 544: 8, 545: 8, 546: 8, 552: 8, 554: 8, 557: 8, 576: 8, 577: 8, 722: 8, 801: 8, 802: 8, 805: 8, 808: 8, 811: 8, 816: 8, 826: 8, 827: 8, 837: 8, 838: 8, 839: 8, 842: 8, 912: 8, 915: 8, 940: 8, 1614: 8, 1617: 8, 1632: 8, 1650: 8, 1657: 8, 1658: 8, 1677: 8, 1697: 8, 1722: 8, 1743: 8, 1759: 8, 1786: 5, 1787: 5, 1788: 8, 1809: 8, 1813: 8, 1817: 8, 1821: 8, 1840: 8, 1848: 8, 1924: 8, 1932: 8, 1952: 8, 1960: 8
}], }],
CAR.IMPREZA_2020: [{
# SUBARU CROSSTREK SPORT 2020
2: 8, 64: 8, 65: 8, 72: 8, 73: 8, 280: 8, 281: 8, 282: 8, 290: 8, 312: 8, 313: 8, 314: 8, 315: 8, 316: 8, 326: 8, 372: 8, 544: 8, 545: 8, 546: 8, 552: 8, 554: 8, 557: 8, 576: 8, 577: 8, 722: 8, 801: 8, 802: 8, 803: 8, 805: 8, 808: 8, 816: 8, 826: 8, 837: 8, 838: 8, 839: 8, 842: 8, 912: 8, 915: 8, 940: 8, 1617: 8, 1632: 8, 1650: 8, 1677: 8, 1697: 8, 1722: 8, 1743: 8, 1759: 8, 1786: 5, 1787: 5, 1788: 8, 1809: 8, 1813: 8, 1817: 8, 1821: 8, 1840: 8, 1848: 8, 1924: 8, 1932: 8, 1952: 8, 1960: 8, 1968: 8, 1976: 8, 2015: 8, 2016: 8, 2024: 8
},
# IMPREZA 2020
{
2: 8, 64: 8, 65: 8, 72: 8, 73: 8, 280: 8, 281: 8, 282: 8, 290: 8, 312: 8, 313: 8, 314: 8, 315: 8, 316: 8, 326: 8, 544: 8, 545: 8, 546: 8, 554: 8, 557: 8, 576: 8, 577: 8, 801: 8, 802: 8, 803: 8, 805: 8, 808: 8, 816: 8, 826: 8, 837: 8, 838: 8, 839: 8, 842: 8, 912: 8, 915: 8, 940: 8, 1614: 8, 1617: 8, 1632: 8, 1657: 8, 1658: 8, 1677: 8, 1697: 8, 1743: 8, 1759: 8, 1786: 5, 1787: 5, 1788: 8, 1809: 8, 1813: 8, 1817: 8, 1821: 8, 1840: 8, 1848: 8, 1924: 8, 1932: 8, 1952: 8, 1960: 8
}],
CAR.FORESTER: [{ CAR.FORESTER: [{
# Forester 2019-2020 # Forester 2019-2020
2: 8, 64: 8, 65: 8, 72: 8, 73: 8, 280: 8, 281: 8, 282: 8, 290: 8, 312: 8, 313: 8, 314: 8, 315: 8, 316: 8, 326: 8, 372: 8, 544: 8, 545: 8, 546: 8, 552: 8, 554: 8, 557: 8, 576: 8, 577: 8, 722: 8, 801: 8, 802: 8, 803: 8, 805: 8, 808: 8, 811: 8, 816: 8, 826: 8, 837: 8, 838: 8, 839: 8, 842: 8, 912: 8, 915: 8, 940: 8, 961: 8, 984: 8, 1614: 8, 1617: 8, 1632: 8, 1650: 8, 1651: 8, 1657: 8, 1658: 8, 1677: 8, 1697: 8, 1698: 8, 1722: 8, 1743: 8, 1759: 8, 1787: 5, 1788: 8, 1809: 8, 1813: 8, 1817: 8, 1821: 8, 1840: 8, 1848: 8, 1924: 8, 1932: 8, 1952: 8, 1960: 8 2: 8, 64: 8, 65: 8, 72: 8, 73: 8, 280: 8, 281: 8, 282: 8, 290: 8, 312: 8, 313: 8, 314: 8, 315: 8, 316: 8, 326: 8, 372: 8, 544: 8, 545: 8, 546: 8, 552: 8, 554: 8, 557: 8, 576: 8, 577: 8, 722: 8, 801: 8, 802: 8, 803: 8, 805: 8, 808: 8, 811: 8, 816: 8, 826: 8, 837: 8, 838: 8, 839: 8, 842: 8, 912: 8, 915: 8, 940: 8, 961: 8, 984: 8, 1614: 8, 1617: 8, 1632: 8, 1650: 8, 1651: 8, 1657: 8, 1658: 8, 1677: 8, 1697: 8, 1698: 8, 1722: 8, 1743: 8, 1759: 8, 1787: 5, 1788: 8, 1809: 8, 1813: 8, 1817: 8, 1821: 8, 1840: 8, 1848: 8, 1924: 8, 1932: 8, 1952: 8, 1960: 8
@ -68,9 +81,57 @@ FINGERPRINTS = {
}], }],
} }
FW_VERSIONS = {
CAR.ASCENT: {
# 2019 Ascent UDM
# Ecu, addr, subaddr: ROM ID
(Ecu.esp, 0x7b0, None): [
b'\xa5 \x19\x02\x00',
],
(Ecu.eps, 0x746, None): [
b'\x85\xc0\xd0\x00',
],
(Ecu.fwdCamera, 0x787, None): [
b'\x00\x00d\xb9\x1f@ \x10',
],
(Ecu.engine, 0x7e0, None): [
b'\xbb,\xa0t\a',
],
(Ecu.transmission, 0x7e1, None): [
b'\x00\xfe\xf7\x00\x00',
],
},
CAR.IMPREZA: {
# 2018 Crosstrek EDM
# 2018 Impreza ADM
# Ecu, addr, subaddr: ROM ID
(Ecu.esp, 0x7b0, None): [
b'\x7a\x94\x3f\x90\x00',
b'\xa2 \x185\x00',
],
(Ecu.eps, 0x746, None): [
b'\x7a\xc0\x0c\x00',
b'z\xc0\b\x00',
],
(Ecu.fwdCamera, 0x787, None): [
b'\x00\x00\x64\xb5\x1f\x40\x20\x0e',
b'\x00\x00d\xdc\x1f@ \x0e',
],
(Ecu.engine, 0x7e0, None): [
b'\xaa\x61\x66\x73\x07',
b'\xbeacr\a',
],
(Ecu.transmission, 0x7e1, None): [
b'\xe3\xe5\x46\x31\x00',
b'\xe4\xe5\x061\x00',
],
},
}
STEER_THRESHOLD = { STEER_THRESHOLD = {
CAR.ASCENT: 80, CAR.ASCENT: 80,
CAR.IMPREZA: 80, CAR.IMPREZA: 80,
CAR.IMPREZA_2020: 80,
CAR.FORESTER: 80, CAR.FORESTER: 80,
CAR.FORESTER_PREGLOBAL: 75, CAR.FORESTER_PREGLOBAL: 75,
CAR.LEGACY_PREGLOBAL: 75, CAR.LEGACY_PREGLOBAL: 75,
@ -81,6 +142,7 @@ STEER_THRESHOLD = {
DBC = { DBC = {
CAR.ASCENT: dbc_dict('subaru_global_2017_generated', None), CAR.ASCENT: dbc_dict('subaru_global_2017_generated', None),
CAR.IMPREZA: dbc_dict('subaru_global_2017_generated', None), CAR.IMPREZA: dbc_dict('subaru_global_2017_generated', None),
CAR.IMPREZA_2020: dbc_dict('subaru_global_2017_generated', None),
CAR.FORESTER: dbc_dict('subaru_global_2017_generated', None), CAR.FORESTER: dbc_dict('subaru_global_2017_generated', None),
CAR.FORESTER_PREGLOBAL: dbc_dict('subaru_forester_2017_generated', None), CAR.FORESTER_PREGLOBAL: dbc_dict('subaru_forester_2017_generated', None),
CAR.LEGACY_PREGLOBAL: dbc_dict('subaru_outback_2015_generated', None), CAR.LEGACY_PREGLOBAL: dbc_dict('subaru_outback_2015_generated', None),

@ -62,4 +62,7 @@ class CarController():
# TODO: HUD control # TODO: HUD control
return can_sends new_actuators = actuators.copy()
new_actuators.steeringAngleDeg = apply_angle
return actuators, can_sends

@ -71,6 +71,6 @@ class CarInterface(CarInterfaceBase):
return self.CS.out return self.CS.out
def apply(self, c): def apply(self, c):
can_sends = self.CC.update(c.enabled, self.CS, self.frame, c.actuators, c.cruiseControl.cancel) ret = self.CC.update(c.enabled, self.CS, self.frame, c.actuators, c.cruiseControl.cancel)
self.frame += 1 self.frame += 1
return can_sends return ret

@ -59,7 +59,7 @@ class TestCarInterfaces(unittest.TestCase):
car_interface.apply(CC) car_interface.apply(CC)
# Test radar interface # Test radar interface
RadarInterface = importlib.import_module('selfdrive.car.%s.radar_interface' % car_params.carName).RadarInterface RadarInterface = importlib.import_module(f'selfdrive.car.{car_params.carName}.radar_interface').RadarInterface
radar_interface = RadarInterface(car_params) radar_interface = RadarInterface(car_params)
assert radar_interface assert radar_interface

@ -19,12 +19,12 @@ class CarController():
self.steer_rate_limited = False self.steer_rate_limited = False
self.packer = CANPacker(dbc_name) self.packer = CANPacker(dbc_name)
self.gas = 0
self.accel = 0
def update(self, enabled, active, CS, frame, actuators, pcm_cancel_cmd, hud_alert, def update(self, enabled, active, CS, frame, actuators, pcm_cancel_cmd, hud_alert,
left_line, right_line, lead, left_lane_depart, right_lane_depart): left_line, right_line, lead, left_lane_depart, right_lane_depart):
# *** compute control surfaces ***
# gas and brake # gas and brake
if CS.CP.enableGasInterceptor and enabled: if CS.CP.enableGasInterceptor and enabled:
MAX_INTERCEPTOR_GAS = 0.5 MAX_INTERCEPTOR_GAS = 0.5
@ -89,20 +89,22 @@ class CarController():
# we can spam can to cancel the system even if we are using lat only control # we can spam can to cancel the system even if we are using lat only control
if (frame % 3 == 0 and CS.CP.openpilotLongitudinalControl) or pcm_cancel_cmd: if (frame % 3 == 0 and CS.CP.openpilotLongitudinalControl) or pcm_cancel_cmd:
lead = lead or CS.out.vEgo < 12. # at low speed we always assume the lead is present do ACC can be engaged lead = lead or CS.out.vEgo < 12. # at low speed we always assume the lead is present so ACC can be engaged
# Lexus IS uses a different cancellation message # Lexus IS uses a different cancellation message
if pcm_cancel_cmd and CS.CP.carFingerprint in [CAR.LEXUS_IS, CAR.LEXUS_RC]: if pcm_cancel_cmd and CS.CP.carFingerprint in [CAR.LEXUS_IS, CAR.LEXUS_RC]:
can_sends.append(create_acc_cancel_command(self.packer)) can_sends.append(create_acc_cancel_command(self.packer))
elif CS.CP.openpilotLongitudinalControl: elif CS.CP.openpilotLongitudinalControl:
can_sends.append(create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.standstill_req, lead, CS.acc_type)) can_sends.append(create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.standstill_req, lead, CS.acc_type))
self.accel = pcm_accel_cmd
else: else:
can_sends.append(create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead, CS.acc_type)) can_sends.append(create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead, CS.acc_type))
if frame % 2 == 0 and CS.CP.enableGasInterceptor: if frame % 2 == 0 and CS.CP.enableGasInterceptor and CS.CP.openpilotLongitudinalControl:
# send exactly zero if gas cmd is zero. Interceptor will send the max between read value and gas cmd. # send exactly zero if gas cmd is zero. Interceptor will send the max between read value and gas cmd.
# This prevents unexpected pedal range rescaling # This prevents unexpected pedal range rescaling
can_sends.append(create_gas_interceptor_command(self.packer, interceptor_gas_cmd, frame // 2)) 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 100Hz but we send asap if:
# - there is something to display # - there is something to display
@ -120,15 +122,19 @@ class CarController():
send_ui = True send_ui = True
if (frame % 100 == 0 or send_ui): if (frame % 100 == 0 or send_ui):
can_sends.append(create_ui_command(self.packer, steer_alert, pcm_cancel_cmd, left_line, right_line, left_lane_depart, right_lane_depart)) can_sends.append(create_ui_command(self.packer, steer_alert, pcm_cancel_cmd, left_line, right_line, left_lane_depart, right_lane_depart, enabled))
if frame % 100 == 0 and CS.CP.enableDsu: if frame % 100 == 0 and CS.CP.enableDsu:
can_sends.append(create_fcw_command(self.packer, fcw_alert)) can_sends.append(create_fcw_command(self.packer, fcw_alert))
#*** static msgs *** # *** static msgs ***
for (addr, cars, bus, fr_step, vl) in STATIC_DSU_MSGS: for (addr, cars, bus, fr_step, vl) in STATIC_DSU_MSGS:
if frame % fr_step == 0 and CS.CP.enableDsu and CS.CP.carFingerprint in cars: if frame % fr_step == 0 and CS.CP.enableDsu and CS.CP.carFingerprint in cars:
can_sends.append(make_can_msg(addr, vl, bus)) can_sends.append(make_can_msg(addr, vl, bus))
return can_sends new_actuators = actuators.copy()
new_actuators.steer = apply_steer / CarControllerParams.STEER_MAX
new_actuators.accel = self.accel
new_actuators.gas = self.gas
return new_actuators, can_sends

@ -57,40 +57,15 @@ class CarInterface(CarInterfaceBase):
ret.mass = 2860. * CV.LB_TO_KG + STD_CARGO_KG # mean between normal and hybrid ret.mass = 2860. * CV.LB_TO_KG + STD_CARGO_KG # mean between normal and hybrid
set_lat_tune(ret.lateralTuning, LatTunes.PID_A) set_lat_tune(ret.lateralTuning, LatTunes.PID_A)
elif candidate == CAR.LEXUS_RX: elif candidate in [CAR.LEXUS_RX, CAR.LEXUS_RXH, CAR.LEXUS_RX_TSS2, CAR.LEXUS_RXH_TSS2]:
stop_and_go = True
ret.wheelbase = 2.79
ret.steerRatio = 14.8
tire_stiffness_factor = 0.5533
ret.mass = 4387. * CV.LB_TO_KG + STD_CARGO_KG
set_lat_tune(ret.lateralTuning, LatTunes.PID_B)
elif candidate == CAR.LEXUS_RXH:
stop_and_go = True stop_and_go = True
ret.wheelbase = 2.79 ret.wheelbase = 2.79
ret.steerRatio = 16. # 14.8 is spec end-to-end ret.steerRatio = 16. # 14.8 is spec end-to-end
tire_stiffness_factor = 0.444 # not optimized yet ret.wheelSpeedFactor = 1.035
tire_stiffness_factor = 0.5533
ret.mass = 4481. * CV.LB_TO_KG + STD_CARGO_KG # mean between min and max ret.mass = 4481. * CV.LB_TO_KG + STD_CARGO_KG # mean between min and max
set_lat_tune(ret.lateralTuning, LatTunes.PID_C) set_lat_tune(ret.lateralTuning, LatTunes.PID_C)
elif candidate == CAR.LEXUS_RX_TSS2:
stop_and_go = True
ret.wheelbase = 2.79
ret.steerRatio = 14.8
tire_stiffness_factor = 0.5533 # not optimized yet
ret.mass = 4387. * CV.LB_TO_KG + STD_CARGO_KG
set_lat_tune(ret.lateralTuning, LatTunes.PID_D)
ret.wheelSpeedFactor = 1.035
elif candidate == CAR.LEXUS_RXH_TSS2:
stop_and_go = True
ret.wheelbase = 2.79
ret.steerRatio = 16.0 # 14.8 is spec end-to-end
tire_stiffness_factor = 0.444 # not optimized yet
ret.mass = 4481.0 * CV.LB_TO_KG + STD_CARGO_KG # mean between min and max
set_lat_tune(ret.lateralTuning, LatTunes.PID_E)
ret.wheelSpeedFactor = 1.035
elif candidate in [CAR.CHR, CAR.CHRH]: elif candidate in [CAR.CHR, CAR.CHRH]:
stop_and_go = True stop_and_go = True
ret.wheelbase = 2.63906 ret.wheelbase = 2.63906
@ -312,12 +287,12 @@ class CarInterface(CarInterfaceBase):
# pass in a car.CarControl # pass in a car.CarControl
# to be called @ 100hz # to be called @ 100hz
def apply(self, c): def apply(self, c):
hud_control = c.hudControl
can_sends = self.CC.update(c.enabled, c.active, self.CS, self.frame, ret = self.CC.update(c.enabled, c.active, self.CS, self.frame,
c.actuators, c.cruiseControl.cancel, c.actuators, c.cruiseControl.cancel,
c.hudControl.visualAlert, c.hudControl.leftLaneVisible, hud_control.visualAlert, hud_control.leftLaneVisible,
c.hudControl.rightLaneVisible, c.hudControl.leadVisible, hud_control.rightLaneVisible, hud_control.leadVisible,
c.hudControl.leftLaneDepart, c.hudControl.rightLaneDepart) hud_control.leftLaneDepart, hud_control.rightLaneDepart)
self.frame += 1 self.frame += 1
return can_sends return ret

@ -67,11 +67,11 @@ def create_fcw_command(packer, fcw):
return packer.make_can_msg("ACC_HUD", 0, values) return packer.make_can_msg("ACC_HUD", 0, values)
def create_ui_command(packer, steer, chime, left_line, right_line, left_lane_depart, right_lane_depart): def create_ui_command(packer, steer, chime, left_line, right_line, left_lane_depart, right_lane_depart, enabled):
values = { values = {
"RIGHT_LINE": 3 if right_lane_depart else 1 if right_line else 2, "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, "LEFT_LINE": 3 if left_lane_depart else 1 if left_line else 2,
"BARRIERS" : 3 if left_lane_depart else 2 if right_lane_depart else 0, "BARRIERS" : 1 if enabled else 0,
"SET_ME_X0C": 0x0c, "SET_ME_X0C": 0x0c,
"SET_ME_X2C": 0x2c, "SET_ME_X2C": 0x2c,
"SET_ME_X38": 0x38, "SET_ME_X38": 0x38,

@ -80,10 +80,6 @@ def set_lat_tune(tune, name):
tune.pid.kpV = [0.2] tune.pid.kpV = [0.2]
tune.pid.kiV = [0.05] tune.pid.kiV = [0.05]
tune.pid.kf = 0.00003 tune.pid.kf = 0.00003
elif name == LatTunes.PID_B:
tune.pid.kpV = [0.6]
tune.pid.kiV = [0.05]
tune.pid.kf = 0.00006
elif name == LatTunes.PID_C: elif name == LatTunes.PID_C:
tune.pid.kpV = [0.6] tune.pid.kpV = [0.6]
tune.pid.kiV = [0.1] tune.pid.kiV = [0.1]
@ -92,10 +88,6 @@ def set_lat_tune(tune, name):
tune.pid.kpV = [0.6] tune.pid.kpV = [0.6]
tune.pid.kiV = [0.1] tune.pid.kiV = [0.1]
tune.pid.kf = 0.00007818594 tune.pid.kf = 0.00007818594
elif name == LatTunes.PID_E:
tune.pid.kpV = [0.6]
tune.pid.kiV = [0.15]
tune.pid.kf = 0.00007818594
elif name == LatTunes.PID_F: elif name == LatTunes.PID_F:
tune.pid.kpV = [0.723] tune.pid.kpV = [0.723]
tune.pid.kiV = [0.0428] tune.pid.kiV = [0.0428]

@ -536,6 +536,7 @@ FW_VERSIONS = {
b'\x018966312P9200\x00\x00\x00\x00', b'\x018966312P9200\x00\x00\x00\x00',
b'\x018966312P9300\x00\x00\x00\x00', b'\x018966312P9300\x00\x00\x00\x00',
b'\x018966312Q2300\x00\x00\x00\x00', b'\x018966312Q2300\x00\x00\x00\x00',
b'\x018966312Q8000\x00\x00\x00\x00',
b'\x018966312R0000\x00\x00\x00\x00', b'\x018966312R0000\x00\x00\x00\x00',
b'\x018966312R0100\x00\x00\x00\x00', b'\x018966312R0100\x00\x00\x00\x00',
b'\x018966312R1000\x00\x00\x00\x00', b'\x018966312R1000\x00\x00\x00\x00',
@ -583,6 +584,7 @@ FW_VERSIONS = {
b'\x01F152612B60\x00\x00\x00\x00\x00\x00', b'\x01F152612B60\x00\x00\x00\x00\x00\x00',
b'\x01F152612B61\x00\x00\x00\x00\x00\x00', b'\x01F152612B61\x00\x00\x00\x00\x00\x00',
b'\x01F152612B71\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'\x01F152612B90\x00\x00\x00\x00\x00\x00',
b'\x01F152612C00\x00\x00\x00\x00\x00\x00', b'\x01F152612C00\x00\x00\x00\x00\x00\x00',
b'F152602191\x00\x00\x00\x00\x00\x00', b'F152602191\x00\x00\x00\x00\x00\x00',
@ -768,6 +770,7 @@ FW_VERSIONS = {
b'\x01896630EB2100\x00\x00\x00\x00', b'\x01896630EB2100\x00\x00\x00\x00',
b'\x01896630EB2200\x00\x00\x00\x00', b'\x01896630EB2200\x00\x00\x00\x00',
b'\x01896630EC4000\x00\x00\x00\x00', b'\x01896630EC4000\x00\x00\x00\x00',
b'\x01896630ED9000\x00\x00\x00\x00',
b'\x01896630EE1000\x00\x00\x00\x00', b'\x01896630EE1000\x00\x00\x00\x00',
], ],
(Ecu.fwdRadar, 0x750, 0xf): [ (Ecu.fwdRadar, 0x750, 0xf): [
@ -797,6 +800,7 @@ FW_VERSIONS = {
b'\x02896630E66000\x00\x00\x00\x00897CF4801001\x00\x00\x00\x00', b'\x02896630E66000\x00\x00\x00\x00897CF4801001\x00\x00\x00\x00',
b'\x02896630EB3000\x00\x00\x00\x00897CF4801001\x00\x00\x00\x00', b'\x02896630EB3000\x00\x00\x00\x00897CF4801001\x00\x00\x00\x00',
b'\x02896630EB3100\x00\x00\x00\x00897CF4801001\x00\x00\x00\x00', b'\x02896630EB3100\x00\x00\x00\x00897CF4801001\x00\x00\x00\x00',
b'\x02896630E66100\x00\x00\x00\x00897CF4801001\x00\x00\x00\x00',
], ],
(Ecu.fwdRadar, 0x750, 0xf): [ (Ecu.fwdRadar, 0x750, 0xf): [
b'\x018821F3301400\x00\x00\x00\x00', b'\x018821F3301400\x00\x00\x00\x00',
@ -1552,6 +1556,7 @@ FW_VERSIONS = {
}, },
CAR.PRIUS_TSS2: { CAR.PRIUS_TSS2: {
(Ecu.engine, 0x700, None): [ (Ecu.engine, 0x700, None): [
b'\x028966347B1000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00',
b'\x028966347C6000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', b'\x028966347C6000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00',
b'\x028966347C8000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', b'\x028966347C8000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00',
b'\x038966347C0000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00897CF4710101\x00\x00\x00\x00', b'\x038966347C0000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00897CF4710101\x00\x00\x00\x00',
@ -1564,6 +1569,7 @@ FW_VERSIONS = {
b'F152647510\x00\x00\x00\x00\x00\x00', b'F152647510\x00\x00\x00\x00\x00\x00',
b'F152647520\x00\x00\x00\x00\x00\x00', b'F152647520\x00\x00\x00\x00\x00\x00',
b'F152647521\x00\x00\x00\x00\x00\x00', b'F152647521\x00\x00\x00\x00\x00\x00',
b'F152647531\x00\x00\x00\x00\x00\x00',
], ],
(Ecu.eps, 0x7a1, None): [ (Ecu.eps, 0x7a1, None): [
b'8965B47070\x00\x00\x00\x00\x00\x00', b'8965B47070\x00\x00\x00\x00\x00\x00',
@ -1636,11 +1642,11 @@ DBC = {
# Toyota/Lexus Safety Sense 2.0 and 2.5 # Toyota/Lexus Safety Sense 2.0 and 2.5
TSS2_CAR = set([CAR.RAV4_TSS2, CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2, CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2, CAR.RAV4H_TSS2, 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.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.MIRAI, CAR.LEXUS_NX_TSS2, CAR.ALPHARD_TSS2}
NO_DSU_CAR = TSS2_CAR | set([CAR.CHR, CAR.CHRH, CAR.CAMRY, CAR.CAMRYH]) NO_DSU_CAR = TSS2_CAR | {CAR.CHR, CAR.CHRH, CAR.CAMRY, CAR.CAMRYH}
# no resume button press required # no resume button press required
NO_STOP_TIMER_CAR = TSS2_CAR | set([CAR.RAV4H, CAR.HIGHLANDERH, CAR.HIGHLANDER, CAR.SIENNA, CAR.LEXUS_ESH]) NO_STOP_TIMER_CAR = TSS2_CAR | {CAR.RAV4H, CAR.HIGHLANDERH, CAR.HIGHLANDER, CAR.SIENNA, CAR.LEXUS_ESH}

@ -110,4 +110,7 @@ class CarController():
self.graButtonStatesToSend = None self.graButtonStatesToSend = None
self.graMsgSentCount = 0 self.graMsgSentCount = 0
return can_sends new_actuators = actuators.copy()
new_actuators.steer = self.apply_steer_last / P.STEER_MAX
return new_actuators, can_sends

@ -152,7 +152,7 @@ class CarInterface(CarInterfaceBase):
ret.wheelbase = 2.84 ret.wheelbase = 2.84
else: else:
raise ValueError("unsupported car %s" % candidate) raise ValueError(f"unsupported car {candidate}")
ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase) ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase)
ret.centerToFront = ret.wheelbase * 0.45 ret.centerToFront = ret.wheelbase * 0.45
@ -216,11 +216,12 @@ class CarInterface(CarInterfaceBase):
return self.CS.out return self.CS.out
def apply(self, c): def apply(self, c):
can_sends = self.CC.update(c.enabled, self.CS, self.frame, self.ext_bus, c.actuators, hud_control = c.hudControl
c.hudControl.visualAlert, ret = self.CC.update(c.enabled, self.CS, self.frame, self.ext_bus, c.actuators,
c.hudControl.leftLaneVisible, hud_control.visualAlert,
c.hudControl.rightLaneVisible, hud_control.leftLaneVisible,
c.hudControl.leftLaneDepart, hud_control.rightLaneVisible,
c.hudControl.rightLaneDepart) hud_control.leftLaneDepart,
hud_control.rightLaneDepart)
self.frame += 1 self.frame += 1
return can_sends return ret

@ -101,19 +101,24 @@ class CAR:
FW_VERSIONS = { FW_VERSIONS = {
CAR.ARTEON_MK1: { CAR.ARTEON_MK1: {
(Ecu.engine, 0x7e0, None): [ (Ecu.engine, 0x7e0, None): [
b'\xf1\x873G0906259F \xf1\x890004',
b'\xf1\x873G0906259P \xf1\x890001', b'\xf1\x873G0906259P \xf1\x890001',
], ],
(Ecu.transmission, 0x7e1, None): [ (Ecu.transmission, 0x7e1, None): [
b'\xf1\x8709G927158L \xf1\x893611', b'\xf1\x8709G927158L \xf1\x893611',
b'\xf1\x870GC300011L \xf1\x891401',
], ],
(Ecu.srs, 0x715, None): [ (Ecu.srs, 0x715, None): [
b'\xf1\x873Q0959655BK\xf1\x890703\xf1\x82\x0e1616001613121177161113772900',
b'\xf1\x873Q0959655DL\xf1\x890732\xf1\x82\0161812141812171105141123052J00', b'\xf1\x873Q0959655DL\xf1\x890732\xf1\x82\0161812141812171105141123052J00',
], ],
(Ecu.eps, 0x712, None): [ (Ecu.eps, 0x712, None): [
b'\xf1\x873Q0909144K \xf1\x895072\xf1\x82\x0571B41815A1',
b'\xf1\x875Q0910143C \xf1\x892211\xf1\x82\00567B0020800', b'\xf1\x875Q0910143C \xf1\x892211\xf1\x82\00567B0020800',
], ],
(Ecu.fwdRadar, 0x757, None): [ (Ecu.fwdRadar, 0x757, None): [
b'\xf1\x872Q0907572T \xf1\x890383', b'\xf1\x872Q0907572T \xf1\x890383',
b'\xf1\x875Q0907572J \xf1\x890654',
], ],
}, },
CAR.ATLAS_MK1: { CAR.ATLAS_MK1: {
@ -292,6 +297,7 @@ FW_VERSIONS = {
b'\xf1\x8704E906024AK\xf1\x899937', b'\xf1\x8704E906024AK\xf1\x899937',
b'\xf1\x8704E906024AS\xf1\x899912', b'\xf1\x8704E906024AS\xf1\x899912',
b'\xf1\x8704E906024B \xf1\x895594', b'\xf1\x8704E906024B \xf1\x895594',
b'\xf1\x8704E906024C \xf1\x899970',
b'\xf1\x8704E906024L \xf1\x895595', b'\xf1\x8704E906024L \xf1\x895595',
b'\xf1\x8704E906024L \xf1\x899970', b'\xf1\x8704E906024L \xf1\x899970',
b'\xf1\x8704E906027MS\xf1\x896223', b'\xf1\x8704E906027MS\xf1\x896223',
@ -306,6 +312,7 @@ FW_VERSIONS = {
], ],
(Ecu.srs, 0x715, None): [ (Ecu.srs, 0x715, None): [
b'\xf1\x875Q0959655AG\xf1\x890336\xf1\x82\02314171231313500314611011630169333463100', b'\xf1\x875Q0959655AG\xf1\x890336\xf1\x82\02314171231313500314611011630169333463100',
b'\xf1\x875Q0959655AG\xf1\x890338\xf1\x82\x1314171231313500314611011630169333463100',
b'\xf1\x875Q0959655BM\xf1\x890403\xf1\x82\02314171231313500314642011650169333463100', b'\xf1\x875Q0959655BM\xf1\x890403\xf1\x82\02314171231313500314642011650169333463100',
b'\xf1\x875Q0959655BM\xf1\x890403\xf1\x82\02314171231313500314643011650169333463100', b'\xf1\x875Q0959655BM\xf1\x890403\xf1\x82\02314171231313500314643011650169333463100',
b'\xf1\x875Q0959655BR\xf1\x890403\xf1\x82\02311170031313300314240011150119333433100', b'\xf1\x875Q0959655BR\xf1\x890403\xf1\x82\02311170031313300314240011150119333433100',
@ -322,6 +329,7 @@ FW_VERSIONS = {
], ],
(Ecu.fwdRadar, 0x757, None): [ (Ecu.fwdRadar, 0x757, None): [
b'\xf1\x875Q0907572N \xf1\x890681', b'\xf1\x875Q0907572N \xf1\x890681',
b'\xf1\x875Q0907572P \xf1\x890682',
b'\xf1\x875Q0907572R \xf1\x890771', b'\xf1\x875Q0907572R \xf1\x890771',
], ],
}, },
@ -741,6 +749,7 @@ FW_VERSIONS = {
(Ecu.engine, 0x7e0, None): [ (Ecu.engine, 0x7e0, None): [
b'\xf1\x8704L906026FP\xf1\x891196', b'\xf1\x8704L906026FP\xf1\x891196',
b'\xf1\x8704L906026KB\xf1\x894071', b'\xf1\x8704L906026KB\xf1\x894071',
b'\xf1\x8704L906026KD\xf1\x894798',
b'\xf1\x873G0906259B \xf1\x890002', b'\xf1\x873G0906259B \xf1\x890002',
b'\xf1\x873G0906264A \xf1\x890002', b'\xf1\x873G0906264A \xf1\x890002',
], ],
@ -748,6 +757,7 @@ FW_VERSIONS = {
b'\xf1\x870CW300042H \xf1\x891601', b'\xf1\x870CW300042H \xf1\x891601',
b'\xf1\x870D9300011T \xf1\x894801', b'\xf1\x870D9300011T \xf1\x894801',
b'\xf1\x870D9300012 \xf1\x894940', b'\xf1\x870D9300012 \xf1\x894940',
b'\xf1\x870GC300043 \xf1\x892301',
], ],
(Ecu.srs, 0x715, None): [ (Ecu.srs, 0x715, None): [
b'\xf1\x875Q0959655AE\xf1\x890130\xf1\x82\022111200111121001121118112231292221111', b'\xf1\x875Q0959655AE\xf1\x890130\xf1\x82\022111200111121001121118112231292221111',
@ -759,6 +769,7 @@ FW_VERSIONS = {
b'\xf1\x875Q0909143M \xf1\x892041\xf1\x820522UZ070303', b'\xf1\x875Q0909143M \xf1\x892041\xf1\x820522UZ070303',
b'\xf1\x875Q0910143B \xf1\x892201\xf1\x82\00563UZ060700', b'\xf1\x875Q0910143B \xf1\x892201\xf1\x82\00563UZ060700',
b'\xf1\x875Q0910143B \xf1\x892201\xf1\x82\x0563UZ060600', b'\xf1\x875Q0910143B \xf1\x892201\xf1\x82\x0563UZ060600',
b'\xf1\x875Q0910143C \xf1\x892211\xf1\x82\x0567UZ070600',
], ],
(Ecu.fwdRadar, 0x757, None): [ (Ecu.fwdRadar, 0x757, None): [
b'\xf1\x873Q0907572B \xf1\x890192', b'\xf1\x873Q0907572B \xf1\x890192',

@ -10,33 +10,19 @@ const int LON_MPC_N = 32;
const float MIN_DRAW_DISTANCE = 10.0; const float MIN_DRAW_DISTANCE = 10.0;
const float MAX_DRAW_DISTANCE = 100.0; const float MAX_DRAW_DISTANCE = 100.0;
template<typename T_SRC, typename T_DST, size_t size> template <typename T, size_t size>
const std::array<T_DST, size> convert_array_to_type(const std::array<T_SRC, size> &src) { constexpr std::array<T, size> build_idxs(float max_val) {
std::array<T_DST, size> dst = {}; std::array<T, size> result{};
for (int i=0; i<size; i++) { for (int i = 0; i < size; ++i) {
dst[i] = src[i]; result[i] = max_val * ((i / (double)(size - 1)) * (i / (double)(size - 1)));
} }
return dst; return result;
} }
const std::array<double, TRAJECTORY_SIZE> T_IDXS = { constexpr auto T_IDXS = build_idxs<double, TRAJECTORY_SIZE>(10.0);
0. , 0.00976562, 0.0390625 , 0.08789062, 0.15625 , constexpr auto T_IDXS_FLOAT = build_idxs<float, TRAJECTORY_SIZE>(10.0);
0.24414062, 0.3515625 , 0.47851562, 0.625 , 0.79101562, constexpr auto X_IDXS = build_idxs<double, TRAJECTORY_SIZE>(192.0);
0.9765625 , 1.18164062, 1.40625 , 1.65039062, 1.9140625 , constexpr auto X_IDXS_FLOAT = build_idxs<float, TRAJECTORY_SIZE>(192.0);
2.19726562, 2.5 , 2.82226562, 3.1640625 , 3.52539062,
3.90625 , 4.30664062, 4.7265625 , 5.16601562, 5.625 ,
6.10351562, 6.6015625 , 7.11914062, 7.65625 , 8.21289062,
8.7890625 , 9.38476562, 10.};
const auto T_IDXS_FLOAT = convert_array_to_type<double, float, TRAJECTORY_SIZE>(T_IDXS);
const std::array<double, TRAJECTORY_SIZE> X_IDXS = {
0. , 0.1875, 0.75 , 1.6875, 3. , 4.6875,
6.75 , 9.1875, 12. , 15.1875, 18.75 , 22.6875,
27. , 31.6875, 36.75 , 42.1875, 48. , 54.1875,
60.75 , 67.6875, 75. , 82.6875, 90.75 , 99.1875,
108. , 117.1875, 126.75 , 136.6875, 147. , 157.6875,
168.75 , 180.1875, 192.};
const auto X_IDXS_FLOAT = convert_array_to_type<double, float, TRAJECTORY_SIZE>(X_IDXS);
const int TICI_CAM_WIDTH = 1928; const int TICI_CAM_WIDTH = 1928;

@ -85,6 +85,7 @@ private:
std::unordered_map<std::string, uint32_t> keys = { std::unordered_map<std::string, uint32_t> keys = {
{"AccessToken", CLEAR_ON_MANAGER_START | DONT_LOG}, {"AccessToken", CLEAR_ON_MANAGER_START | DONT_LOG},
{"AthenadPid", PERSISTENT}, {"AthenadPid", PERSISTENT},
{"AthenadUploadQueue", PERSISTENT},
{"BootedOnroad", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_OFF}, {"BootedOnroad", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_OFF},
{"CalibrationParams", PERSISTENT}, {"CalibrationParams", PERSISTENT},
{"CarBatteryCapacity", PERSISTENT}, {"CarBatteryCapacity", PERSISTENT},

@ -20,6 +20,8 @@
#include <sched.h> #include <sched.h>
#endif // __linux__ #endif // __linux__
namespace util {
void set_thread_name(const char* name) { void set_thread_name(const char* name) {
#ifdef __linux__ #ifdef __linux__
// pthread_setname_np is dumb (fails instead of truncates) // pthread_setname_np is dumb (fails instead of truncates)
@ -56,8 +58,6 @@ int set_core_affinity(std::vector<int> cores) {
#endif #endif
} }
namespace util {
std::string read_file(const std::string& fn) { std::string read_file(const std::string& fn) {
std::ifstream f(fn, std::ios::binary | std::ios::in); std::ifstream f(fn, std::ios::binary | std::ios::in);
if (f.is_open()) { if (f.is_open()) {

@ -37,13 +37,12 @@ const double MS_TO_MPH = MS_TO_KPH * KM_TO_MILE;
const double METER_TO_MILE = KM_TO_MILE / 1000.0; const double METER_TO_MILE = KM_TO_MILE / 1000.0;
const double METER_TO_FOOT = 3.28084; const double METER_TO_FOOT = 3.28084;
void set_thread_name(const char* name); namespace util {
void set_thread_name(const char* name);
int set_realtime_priority(int level); int set_realtime_priority(int level);
int set_core_affinity(std::vector<int> cores); int set_core_affinity(std::vector<int> cores);
namespace util {
// ***** Time helpers ***** // ***** Time helpers *****
struct tm get_time(); struct tm get_time();
bool time_valid(struct tm sys_time); bool time_valid(struct tm sys_time);

@ -1 +1 @@
#define COMMA_VERSION "0.8.12" #define COMMA_VERSION "0.8.13"

@ -28,6 +28,7 @@ from selfdrive.locationd.calibrationd import Calibration
from selfdrive.hardware import HARDWARE, TICI, EON from selfdrive.hardware import HARDWARE, TICI, EON
from selfdrive.manager.process_config import managed_processes from selfdrive.manager.process_config import managed_processes
SOFT_DISABLE_TIME = 3 # seconds
LDW_MIN_SPEED = 31 * CV.MPH_TO_MS LDW_MIN_SPEED = 31 * CV.MPH_TO_MS
LANE_DEPARTURE_THRESHOLD = 0.1 LANE_DEPARTURE_THRESHOLD = 0.1
STEER_ANGLE_SATURATION_TIMEOUT = 1.0 / DT_CTRL STEER_ANGLE_SATURATION_TIMEOUT = 1.0 / DT_CTRL
@ -149,7 +150,7 @@ class Controls:
self.v_cruise_kph_last = 0 self.v_cruise_kph_last = 0
self.mismatch_counter = 0 self.mismatch_counter = 0
self.cruise_mismatch_counter = 0 self.cruise_mismatch_counter = 0
self.can_error_counter = 0 self.can_rcv_error_counter = 0
self.last_blinker_frame = 0 self.last_blinker_frame = 0
self.saturated_count = 0 self.saturated_count = 0
self.distance_traveled = 0 self.distance_traveled = 0
@ -158,6 +159,7 @@ class Controls:
self.current_alert_types = [ET.PERMANENT] self.current_alert_types = [ET.PERMANENT]
self.logged_comm_issue = False self.logged_comm_issue = False
self.button_timers = {ButtonEvent.Type.decelCruise: 0, ButtonEvent.Type.accelCruise: 0} self.button_timers = {ButtonEvent.Type.decelCruise: 0, ButtonEvent.Type.accelCruise: 0}
self.last_actuators = car.CarControl.Actuators.new_message()
# TODO: no longer necessary, aside from process replay # TODO: no longer necessary, aside from process replay
self.sm['liveParameters'].valid = True self.sm['liveParameters'].valid = True
@ -251,7 +253,7 @@ class Controls:
LaneChangeState.laneChangeFinishing]: LaneChangeState.laneChangeFinishing]:
self.events.add(EventName.laneChange) self.events.add(EventName.laneChange)
if self.can_rcv_error or not CS.canValid: if not CS.canValid:
self.events.add(EventName.canError) self.events.add(EventName.canError)
for i, pandaState in enumerate(self.sm['pandaStates']): for i, pandaState in enumerate(self.sm['pandaStates']):
@ -271,7 +273,7 @@ class Controls:
self.events.add(EventName.radarFault) self.events.add(EventName.radarFault)
elif not self.sm.valid["pandaStates"]: elif not self.sm.valid["pandaStates"]:
self.events.add(EventName.usbError) self.events.add(EventName.usbError)
elif not self.sm.all_alive_and_valid(): elif not self.sm.all_alive_and_valid() or self.can_rcv_error:
self.events.add(EventName.commIssue) self.events.add(EventName.commIssue)
if not self.logged_comm_issue: if not self.logged_comm_issue:
invalid = [s for s, valid in self.sm.valid.items() if not valid] invalid = [s for s, valid in self.sm.valid.items() if not valid]
@ -300,7 +302,7 @@ class Controls:
# Check for mismatch between openpilot and car's PCM # Check for mismatch between openpilot and car's PCM
cruise_mismatch = CS.cruiseState.enabled and (not self.enabled or not self.CP.pcmCruise) cruise_mismatch = CS.cruiseState.enabled and (not self.enabled or not self.CP.pcmCruise)
self.cruise_mismatch_counter = self.cruise_mismatch_counter + 1 if cruise_mismatch else 0 self.cruise_mismatch_counter = self.cruise_mismatch_counter + 1 if cruise_mismatch else 0
if self.cruise_mismatch_counter > int(1. / DT_CTRL): if self.cruise_mismatch_counter > int(3. / DT_CTRL):
self.events.add(EventName.cruiseMismatch) self.events.add(EventName.cruiseMismatch)
# Check for FCW # Check for FCW
@ -344,7 +346,7 @@ class Controls:
self.events.add(EventName.localizerMalfunction) self.events.add(EventName.localizerMalfunction)
# Check if all manager processes are running # Check if all manager processes are running
not_running = set(p.name for p in self.sm['managerState'].processes if not p.running) not_running = {p.name for p in self.sm['managerState'].processes if not p.running}
if self.sm.rcv_frame['managerState'] and (not_running - IGNORE_PROCESSES): if self.sm.rcv_frame['managerState'] and (not_running - IGNORE_PROCESSES):
self.events.add(EventName.processNotRunning) self.events.add(EventName.processNotRunning)
@ -376,7 +378,7 @@ class Controls:
# Check for CAN timeout # Check for CAN timeout
if not can_strs: if not can_strs:
self.can_error_counter += 1 self.can_rcv_error_counter += 1
self.can_rcv_error = True self.can_rcv_error = True
else: else:
self.can_rcv_error = False self.can_rcv_error = False
@ -430,7 +432,7 @@ class Controls:
if self.state == State.enabled: if self.state == State.enabled:
if self.events.any(ET.SOFT_DISABLE): if self.events.any(ET.SOFT_DISABLE):
self.state = State.softDisabling self.state = State.softDisabling
self.soft_disable_timer = int(3 / DT_CTRL) self.soft_disable_timer = int(SOFT_DISABLE_TIME / DT_CTRL)
self.current_alert_types.append(ET.SOFT_DISABLE) self.current_alert_types.append(ET.SOFT_DISABLE)
# SOFT DISABLING # SOFT DISABLING
@ -509,7 +511,7 @@ class Controls:
lat_plan.psis, lat_plan.psis,
lat_plan.curvatures, lat_plan.curvatures,
lat_plan.curvatureRates) lat_plan.curvatureRates)
actuators.steer, actuators.steeringAngleDeg, lac_log = self.LaC.update(lat_active, CS, self.CP, self.VM, params, actuators.steer, actuators.steeringAngleDeg, lac_log = self.LaC.update(lat_active, CS, self.CP, self.VM, params, self.last_actuators,
desired_curvature, desired_curvature_rate) desired_curvature, desired_curvature_rate)
else: else:
lac_log = log.ControlsState.LateralDebugState.new_message() lac_log = log.ControlsState.LateralDebugState.new_message()
@ -577,59 +579,66 @@ class Controls:
CC.active = self.active CC.active = self.active
CC.actuators = actuators CC.actuators = actuators
if len(self.sm['liveLocationKalman'].orientationNED.value) > 2: orientation_value = self.sm['liveLocationKalman'].orientationNED.value
CC.roll = self.sm['liveLocationKalman'].orientationNED.value[0] if len(orientation_value) > 2:
CC.pitch = self.sm['liveLocationKalman'].orientationNED.value[1] CC.roll = orientation_value[0]
CC.pitch = orientation_value[1]
CC.cruiseControl.cancel = CS.cruiseState.enabled and (not self.enabled or not self.CP.pcmCruise) CC.cruiseControl.cancel = CS.cruiseState.enabled and (not self.enabled or not self.CP.pcmCruise)
if self.joystick_mode and self.sm.rcv_frame['testJoystick'] > 0 and self.sm['testJoystick'].buttons[0]: if self.joystick_mode and self.sm.rcv_frame['testJoystick'] > 0 and self.sm['testJoystick'].buttons[0]:
CC.cruiseControl.cancel = True CC.cruiseControl.cancel = True
CC.hudControl.setSpeed = float(self.v_cruise_kph * CV.KPH_TO_MS) hudControl = CC.hudControl
CC.hudControl.speedVisible = self.enabled hudControl.setSpeed = float(self.v_cruise_kph * CV.KPH_TO_MS)
CC.hudControl.lanesVisible = self.enabled hudControl.speedVisible = self.enabled
CC.hudControl.leadVisible = self.sm['longitudinalPlan'].hasLead hudControl.lanesVisible = self.enabled
hudControl.leadVisible = self.sm['longitudinalPlan'].hasLead
CC.hudControl.rightLaneVisible = True hudControl.rightLaneVisible = True
CC.hudControl.leftLaneVisible = True hudControl.leftLaneVisible = True
recent_blinker = (self.sm.frame - self.last_blinker_frame) * DT_CTRL < 5.0 # 5s blinker cooldown recent_blinker = (self.sm.frame - self.last_blinker_frame) * DT_CTRL < 5.0 # 5s blinker cooldown
ldw_allowed = self.is_ldw_enabled and CS.vEgo > LDW_MIN_SPEED and not recent_blinker \ ldw_allowed = self.is_ldw_enabled and CS.vEgo > LDW_MIN_SPEED and not recent_blinker \
and not self.active and self.sm['liveCalibration'].calStatus == Calibration.CALIBRATED and not self.active and self.sm['liveCalibration'].calStatus == Calibration.CALIBRATED
meta = self.sm['modelV2'].meta model_v2 = self.sm['modelV2']
if len(meta.desirePrediction) and ldw_allowed: desire_prediction = model_v2.meta.desirePrediction
if len(desire_prediction) and ldw_allowed:
right_lane_visible = self.sm['lateralPlan'].rProb > 0.5 right_lane_visible = self.sm['lateralPlan'].rProb > 0.5
left_lane_visible = self.sm['lateralPlan'].lProb > 0.5 left_lane_visible = self.sm['lateralPlan'].lProb > 0.5
l_lane_change_prob = meta.desirePrediction[Desire.laneChangeLeft - 1] l_lane_change_prob = desire_prediction[Desire.laneChangeLeft - 1]
r_lane_change_prob = meta.desirePrediction[Desire.laneChangeRight - 1] r_lane_change_prob = desire_prediction[Desire.laneChangeRight - 1]
l_lane_close = left_lane_visible and (self.sm['modelV2'].laneLines[1].y[0] > -(1.08 + CAMERA_OFFSET))
r_lane_close = right_lane_visible and (self.sm['modelV2'].laneLines[2].y[0] < (1.08 - CAMERA_OFFSET))
CC.hudControl.leftLaneDepart = bool(l_lane_change_prob > LANE_DEPARTURE_THRESHOLD and l_lane_close) lane_lines = model_v2.laneLines
CC.hudControl.rightLaneDepart = bool(r_lane_change_prob > LANE_DEPARTURE_THRESHOLD and r_lane_close) l_lane_close = left_lane_visible and (lane_lines[1].y[0] > -(1.08 + CAMERA_OFFSET))
r_lane_close = right_lane_visible and (lane_lines[2].y[0] < (1.08 - CAMERA_OFFSET))
if CC.hudControl.rightLaneDepart or CC.hudControl.leftLaneDepart: hudControl.leftLaneDepart = bool(l_lane_change_prob > LANE_DEPARTURE_THRESHOLD and l_lane_close)
hudControl.rightLaneDepart = bool(r_lane_change_prob > LANE_DEPARTURE_THRESHOLD and r_lane_close)
if hudControl.rightLaneDepart or hudControl.leftLaneDepart:
self.events.add(EventName.ldw) self.events.add(EventName.ldw)
clear_event = ET.WARNING if ET.WARNING not in self.current_alert_types else None clear_event = ET.WARNING if ET.WARNING not in self.current_alert_types else None
alerts = self.events.create_alerts(self.current_alert_types, [self.CP, self.sm, self.is_metric]) 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) self.AM.add_many(self.sm.frame, alerts)
self.AM.process_alerts(self.sm.frame, clear_event) self.AM.process_alerts(self.sm.frame, clear_event)
CC.hudControl.visualAlert = self.AM.visual_alert hudControl.visualAlert = self.AM.visual_alert
if not self.read_only and self.initialized: if not self.read_only and self.initialized:
# send car controls over can # send car controls over can
can_sends = self.CI.apply(CC) self.last_actuators, can_sends = self.CI.apply(CC)
self.pm.send('sendcan', can_list_to_can_capnp(can_sends, msgtype='sendcan', valid=CS.canValid)) self.pm.send('sendcan', can_list_to_can_capnp(can_sends, msgtype='sendcan', valid=CS.canValid))
CC.actuatorsOutput = self.last_actuators
force_decel = (self.sm['driverMonitoringState'].awarenessStatus < 0.) or \ force_decel = (self.sm['driverMonitoringState'].awarenessStatus < 0.) or \
(self.state == State.softDisabling) (self.state == State.softDisabling)
# Curvature & Steering angle # Curvature & Steering angle
params = self.sm['liveParameters'] params = self.sm['liveParameters']
steer_angle_without_offset = math.radians(CS.steeringAngleDeg - params.angleOffsetAverageDeg)
curvature = -self.VM.calc_curvature(steer_angle_without_offset, CS.vEgo) steer_angle_without_offset = math.radians(CS.steeringAngleDeg - params.angleOffsetDeg)
curvature = -self.VM.calc_curvature(steer_angle_without_offset, CS.vEgo, params.roll)
# controlsState # controlsState
dat = messaging.new_message('controlsState') dat = messaging.new_message('controlsState')
@ -659,18 +668,20 @@ class Controls:
controlsState.cumLagMs = -self.rk.remaining * 1000. controlsState.cumLagMs = -self.rk.remaining * 1000.
controlsState.startMonoTime = int(start_time * 1e9) controlsState.startMonoTime = int(start_time * 1e9)
controlsState.forceDecel = bool(force_decel) controlsState.forceDecel = bool(force_decel)
controlsState.canErrorCounter = self.can_error_counter controlsState.canErrorCounter = self.can_rcv_error_counter
lat_tuning = self.CP.lateralTuning.which()
if self.joystick_mode: if self.joystick_mode:
controlsState.lateralControlState.debugState = lac_log controlsState.lateralControlState.debugState = lac_log
elif self.CP.steerControlType == car.CarParams.SteerControlType.angle: elif self.CP.steerControlType == car.CarParams.SteerControlType.angle:
controlsState.lateralControlState.angleState = lac_log controlsState.lateralControlState.angleState = lac_log
elif self.CP.lateralTuning.which() == 'pid': elif lat_tuning == 'pid':
controlsState.lateralControlState.pidState = lac_log controlsState.lateralControlState.pidState = lac_log
elif self.CP.lateralTuning.which() == 'lqr': elif lat_tuning == 'lqr':
controlsState.lateralControlState.lqrState = lac_log controlsState.lateralControlState.lqrState = lac_log
elif self.CP.lateralTuning.which() == 'indi': elif lat_tuning == 'indi':
controlsState.lateralControlState.indiState = lac_log controlsState.lateralControlState.indiState = lac_log
self.pm.send('controlsState', dat) self.pm.send('controlsState', dat)
# carState # carState

@ -1,5 +1,5 @@
from enum import IntEnum from enum import IntEnum
from typing import Dict, Union, Callable, Any from typing import Dict, Union, Callable, List, Optional
from cereal import log, car from cereal import log, car
import cereal.messaging as messaging import cereal.messaging as messaging
@ -42,33 +42,33 @@ EVENT_NAME = {v: k for k, v in EventName.schema.enumerants.items()}
class Events: class Events:
def __init__(self): def __init__(self):
self.events = [] self.events: List[int] = []
self.static_events = [] self.static_events: List[int] = []
self.events_prev = dict.fromkeys(EVENTS.keys(), 0) self.events_prev = dict.fromkeys(EVENTS.keys(), 0)
@property @property
def names(self): def names(self) -> List[int]:
return self.events return self.events
def __len__(self): def __len__(self) -> int:
return len(self.events) return len(self.events)
def add(self, event_name, static=False): def add(self, event_name: int, static: bool=False) -> None:
if static: if static:
self.static_events.append(event_name) self.static_events.append(event_name)
self.events.append(event_name) self.events.append(event_name)
def clear(self): def clear(self) -> None:
self.events_prev = {k: (v + 1 if k in self.events else 0) for k, v in self.events_prev.items()} self.events_prev = {k: (v + 1 if k in self.events else 0) for k, v in self.events_prev.items()}
self.events = self.static_events.copy() self.events = self.static_events.copy()
def any(self, event_type): def any(self, event_type: str) -> bool:
for e in self.events: for e in self.events:
if event_type in EVENTS.get(e, {}).keys(): if event_type in EVENTS.get(e, {}).keys():
return True return True
return False return False
def create_alerts(self, event_types, callback_args=None): def create_alerts(self, event_types: List[str], callback_args=None):
if callback_args is None: if callback_args is None:
callback_args = [] callback_args = []
@ -129,7 +129,7 @@ class Alert:
self.creation_delay = creation_delay self.creation_delay = creation_delay
self.alert_type = "" self.alert_type = ""
self.event_type = None self.event_type: Optional[str] = None
def __str__(self) -> str: def __str__(self) -> str:
return f"{self.alert_text_1}/{self.alert_text_2} {self.priority} {self.visual_alert} {self.audible_alert}" return f"{self.alert_text_1}/{self.alert_text_2} {self.priority} {self.visual_alert} {self.audible_alert}"
@ -139,14 +139,14 @@ class Alert:
class NoEntryAlert(Alert): class NoEntryAlert(Alert):
def __init__(self, alert_text_2, visual_alert=VisualAlert.none): def __init__(self, alert_text_2: str, visual_alert: car.CarControl.HUDControl.VisualAlert=VisualAlert.none):
super().__init__("openpilot Unavailable", alert_text_2, AlertStatus.normal, super().__init__("openpilot Unavailable", alert_text_2, AlertStatus.normal,
AlertSize.mid, Priority.LOW, visual_alert, AlertSize.mid, Priority.LOW, visual_alert,
AudibleAlert.refuse, 3.) AudibleAlert.refuse, 3.)
class SoftDisableAlert(Alert): class SoftDisableAlert(Alert):
def __init__(self, alert_text_2): def __init__(self, alert_text_2: str):
super().__init__("TAKE CONTROL IMMEDIATELY", alert_text_2, super().__init__("TAKE CONTROL IMMEDIATELY", alert_text_2,
AlertStatus.userPrompt, AlertSize.full, AlertStatus.userPrompt, AlertSize.full,
Priority.MID, VisualAlert.steerRequired, Priority.MID, VisualAlert.steerRequired,
@ -155,13 +155,13 @@ class SoftDisableAlert(Alert):
# less harsh version of SoftDisable, where the condition is user-triggered # less harsh version of SoftDisable, where the condition is user-triggered
class UserSoftDisableAlert(SoftDisableAlert): class UserSoftDisableAlert(SoftDisableAlert):
def __init__(self, alert_text_2): def __init__(self, alert_text_2: str):
super().__init__(alert_text_2), super().__init__(alert_text_2),
self.alert_text_1 = "openpilot will disengage" self.alert_text_1 = "openpilot will disengage"
class ImmediateDisableAlert(Alert): class ImmediateDisableAlert(Alert):
def __init__(self, alert_text_2): def __init__(self, alert_text_2: str):
super().__init__("TAKE CONTROL IMMEDIATELY", alert_text_2, super().__init__("TAKE CONTROL IMMEDIATELY", alert_text_2,
AlertStatus.critical, AlertSize.full, AlertStatus.critical, AlertSize.full,
Priority.HIGHEST, VisualAlert.steerRequired, Priority.HIGHEST, VisualAlert.steerRequired,
@ -198,11 +198,31 @@ def get_display_speed(speed_ms: float, metric: bool) -> str:
# ********** alert callback functions ********** # ********** alert callback functions **********
def below_engage_speed_alert(CP: car.CarParams, sm: messaging.SubMaster, metric: bool) -> Alert:
AlertCallbackType = Callable[[car.CarParams, messaging.SubMaster, bool, int], Alert]
def soft_disable_alert(alert_text_2: str) -> AlertCallbackType:
def func(CP: car.CarParams, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert:
if soft_disable_time < int(0.5 / DT_CTRL):
return ImmediateDisableAlert(alert_text_2)
return SoftDisableAlert(alert_text_2)
return func
def user_soft_disable_alert(alert_text_2: str) -> AlertCallbackType:
def func(CP: car.CarParams, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert:
if soft_disable_time < int(0.5 / DT_CTRL):
return ImmediateDisableAlert(alert_text_2)
return UserSoftDisableAlert(alert_text_2)
return func
def below_engage_speed_alert(CP: car.CarParams, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert:
return NoEntryAlert(f"Speed Below {get_display_speed(CP.minEnableSpeed, metric)}") return NoEntryAlert(f"Speed Below {get_display_speed(CP.minEnableSpeed, metric)}")
def below_steer_speed_alert(CP: car.CarParams, sm: messaging.SubMaster, metric: bool) -> Alert: def below_steer_speed_alert(CP: car.CarParams, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert:
return Alert( return Alert(
f"Steer Unavailable Below {get_display_speed(CP.minSteerSpeed, metric)}", f"Steer Unavailable Below {get_display_speed(CP.minSteerSpeed, metric)}",
"", "",
@ -210,7 +230,7 @@ def below_steer_speed_alert(CP: car.CarParams, sm: messaging.SubMaster, metric:
Priority.MID, VisualAlert.steerRequired, AudibleAlert.prompt, 0.4) Priority.MID, VisualAlert.steerRequired, AudibleAlert.prompt, 0.4)
def calibration_incomplete_alert(CP: car.CarParams, sm: messaging.SubMaster, metric: bool) -> Alert: def calibration_incomplete_alert(CP: car.CarParams, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert:
return Alert( return Alert(
"Calibration in Progress: %d%%" % sm['liveCalibration'].calPerc, "Calibration in Progress: %d%%" % sm['liveCalibration'].calPerc,
f"Drive Above {get_display_speed(MIN_SPEED_FILTER, metric)}", f"Drive Above {get_display_speed(MIN_SPEED_FILTER, metric)}",
@ -218,7 +238,7 @@ def calibration_incomplete_alert(CP: car.CarParams, sm: messaging.SubMaster, met
Priority.LOWEST, VisualAlert.none, AudibleAlert.none, .2) Priority.LOWEST, VisualAlert.none, AudibleAlert.none, .2)
def no_gps_alert(CP: car.CarParams, sm: messaging.SubMaster, metric: bool) -> Alert: def no_gps_alert(CP: car.CarParams, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert:
gps_integrated = sm['peripheralState'].pandaType in [log.PandaState.PandaType.uno, log.PandaState.PandaType.dos] gps_integrated = sm['peripheralState'].pandaType in [log.PandaState.PandaType.uno, log.PandaState.PandaType.dos]
return Alert( return Alert(
"Poor GPS reception", "Poor GPS reception",
@ -227,25 +247,28 @@ def no_gps_alert(CP: car.CarParams, sm: messaging.SubMaster, metric: bool) -> Al
Priority.LOWER, VisualAlert.none, AudibleAlert.none, .2, creation_delay=300.) Priority.LOWER, VisualAlert.none, AudibleAlert.none, .2, creation_delay=300.)
def wrong_car_mode_alert(CP: car.CarParams, sm: messaging.SubMaster, metric: bool) -> Alert: def wrong_car_mode_alert(CP: car.CarParams, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert:
text = "Cruise Mode Disabled" text = "Cruise Mode Disabled"
if CP.carName == "honda": if CP.carName == "honda":
text = "Main Switch Off" text = "Main Switch Off"
return NoEntryAlert(text) return NoEntryAlert(text)
def joystick_alert(CP: car.CarParams, sm: messaging.SubMaster, metric: bool) -> Alert: def joystick_alert(CP: car.CarParams, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert:
axes = sm['testJoystick'].axes axes = sm['testJoystick'].axes
gb, steer = list(axes)[:2] if len(axes) else (0., 0.) gb, steer = list(axes)[:2] if len(axes) else (0., 0.)
vals = f"Gas: {round(gb * 100.)}%, Steer: {round(steer * 100.)}%" vals = f"Gas: {round(gb * 100.)}%, Steer: {round(steer * 100.)}%"
return NormalPermanentAlert("Joystick Mode", vals) return NormalPermanentAlert("Joystick Mode", vals)
EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, bool], Alert]]]] = {
EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = {
# ********** events with no alerts ********** # ********** events with no alerts **********
EventName.stockFcw: {}, EventName.stockFcw: {},
EventName.lkasDisabled: {},
# ********** events only containing alerts displayed in all states ********** # ********** events only containing alerts displayed in all states **********
EventName.joystickDebug: { EventName.joystickDebug: {
@ -358,7 +381,7 @@ EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, boo
# bad alignment or bad sensor data. If this happens consistently consider creating an issue on GitHub # bad alignment or bad sensor data. If this happens consistently consider creating an issue on GitHub
EventName.vehicleModelInvalid: { EventName.vehicleModelInvalid: {
ET.NO_ENTRY: NoEntryAlert("Vehicle Parameter Identification Failed"), ET.NO_ENTRY: NoEntryAlert("Vehicle Parameter Identification Failed"),
ET.SOFT_DISABLE: SoftDisableAlert("Vehicle Parameter Identification Failed"), ET.SOFT_DISABLE: soft_disable_alert("Vehicle Parameter Identification Failed"),
}, },
EventName.steerTempUnavailableSilent: { EventName.steerTempUnavailableSilent: {
@ -544,7 +567,7 @@ EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, boo
}, },
EventName.steerTempUnavailable: { EventName.steerTempUnavailable: {
ET.SOFT_DISABLE: SoftDisableAlert("Steering Temporarily Unavailable"), ET.SOFT_DISABLE: soft_disable_alert("Steering Temporarily Unavailable"),
ET.NO_ENTRY: NoEntryAlert("Steering Temporarily Unavailable"), ET.NO_ENTRY: NoEntryAlert("Steering Temporarily Unavailable"),
}, },
@ -581,12 +604,12 @@ EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, boo
EventName.overheat: { EventName.overheat: {
ET.PERMANENT: NormalPermanentAlert("System Overheated"), ET.PERMANENT: NormalPermanentAlert("System Overheated"),
ET.SOFT_DISABLE: SoftDisableAlert("System Overheated"), ET.SOFT_DISABLE: soft_disable_alert("System Overheated"),
ET.NO_ENTRY: NoEntryAlert("System Overheated"), ET.NO_ENTRY: NoEntryAlert("System Overheated"),
}, },
EventName.wrongGear: { EventName.wrongGear: {
ET.SOFT_DISABLE: UserSoftDisableAlert("Gear not D"), ET.SOFT_DISABLE: user_soft_disable_alert("Gear not D"),
ET.NO_ENTRY: NoEntryAlert("Gear not D"), ET.NO_ENTRY: NoEntryAlert("Gear not D"),
}, },
@ -597,33 +620,33 @@ EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, boo
# See https://comma.ai/setup for more information # See https://comma.ai/setup for more information
EventName.calibrationInvalid: { EventName.calibrationInvalid: {
ET.PERMANENT: NormalPermanentAlert("Calibration Invalid", "Remount Device and Recalibrate"), ET.PERMANENT: NormalPermanentAlert("Calibration Invalid", "Remount Device and Recalibrate"),
ET.SOFT_DISABLE: SoftDisableAlert("Calibration Invalid: Remount Device & Recalibrate"), ET.SOFT_DISABLE: soft_disable_alert("Calibration Invalid: Remount Device & Recalibrate"),
ET.NO_ENTRY: NoEntryAlert("Calibration Invalid: Remount Device & Recalibrate"), ET.NO_ENTRY: NoEntryAlert("Calibration Invalid: Remount Device & Recalibrate"),
}, },
EventName.calibrationIncomplete: { EventName.calibrationIncomplete: {
ET.PERMANENT: calibration_incomplete_alert, ET.PERMANENT: calibration_incomplete_alert,
ET.SOFT_DISABLE: SoftDisableAlert("Calibration in Progress"), ET.SOFT_DISABLE: soft_disable_alert("Calibration in Progress"),
ET.NO_ENTRY: NoEntryAlert("Calibration in Progress"), ET.NO_ENTRY: NoEntryAlert("Calibration in Progress"),
}, },
EventName.doorOpen: { EventName.doorOpen: {
ET.SOFT_DISABLE: UserSoftDisableAlert("Door Open"), ET.SOFT_DISABLE: user_soft_disable_alert("Door Open"),
ET.NO_ENTRY: NoEntryAlert("Door Open"), ET.NO_ENTRY: NoEntryAlert("Door Open"),
}, },
EventName.seatbeltNotLatched: { EventName.seatbeltNotLatched: {
ET.SOFT_DISABLE: UserSoftDisableAlert("Seatbelt Unlatched"), ET.SOFT_DISABLE: user_soft_disable_alert("Seatbelt Unlatched"),
ET.NO_ENTRY: NoEntryAlert("Seatbelt Unlatched"), ET.NO_ENTRY: NoEntryAlert("Seatbelt Unlatched"),
}, },
EventName.espDisabled: { EventName.espDisabled: {
ET.SOFT_DISABLE: SoftDisableAlert("ESP Off"), ET.SOFT_DISABLE: soft_disable_alert("ESP Off"),
ET.NO_ENTRY: NoEntryAlert("ESP Off"), ET.NO_ENTRY: NoEntryAlert("ESP Off"),
}, },
EventName.lowBattery: { EventName.lowBattery: {
ET.SOFT_DISABLE: SoftDisableAlert("Low Battery"), ET.SOFT_DISABLE: soft_disable_alert("Low Battery"),
ET.NO_ENTRY: NoEntryAlert("Low Battery"), ET.NO_ENTRY: NoEntryAlert("Low Battery"),
}, },
@ -632,7 +655,7 @@ EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, boo
# is thrown. This can mean a service crashed, did not broadcast a message for # is thrown. This can mean a service crashed, did not broadcast a message for
# ten times the regular interval, or the average interval is more than 10% too high. # ten times the regular interval, or the average interval is more than 10% too high.
EventName.commIssue: { EventName.commIssue: {
ET.SOFT_DISABLE: SoftDisableAlert("Communication Issue between Processes"), ET.SOFT_DISABLE: soft_disable_alert("Communication Issue between Processes"),
ET.NO_ENTRY: NoEntryAlert("Communication Issue between Processes"), ET.NO_ENTRY: NoEntryAlert("Communication Issue between Processes"),
}, },
@ -642,7 +665,7 @@ EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, boo
}, },
EventName.radarFault: { EventName.radarFault: {
ET.SOFT_DISABLE: SoftDisableAlert("Radar Error: Restart the Car"), ET.SOFT_DISABLE: soft_disable_alert("Radar Error: Restart the Car"),
ET.NO_ENTRY: NoEntryAlert("Radar Error: Restart the Car"), ET.NO_ENTRY: NoEntryAlert("Radar Error: Restart the Car"),
}, },
@ -650,7 +673,7 @@ EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, boo
# is not processing frames fast enough they have to be dropped. This alert is # is not processing frames fast enough they have to be dropped. This alert is
# thrown when over 20% of frames are dropped. # thrown when over 20% of frames are dropped.
EventName.modeldLagging: { EventName.modeldLagging: {
ET.SOFT_DISABLE: SoftDisableAlert("Driving model lagging"), ET.SOFT_DISABLE: soft_disable_alert("Driving model lagging"),
ET.NO_ENTRY: NoEntryAlert("Driving model lagging"), ET.NO_ENTRY: NoEntryAlert("Driving model lagging"),
}, },
@ -660,25 +683,25 @@ EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, boo
# usually means the model has trouble understanding the scene. This is used # usually means the model has trouble understanding the scene. This is used
# as a heuristic to warn the driver. # as a heuristic to warn the driver.
EventName.posenetInvalid: { EventName.posenetInvalid: {
ET.SOFT_DISABLE: SoftDisableAlert("Model Output Uncertain"), ET.SOFT_DISABLE: soft_disable_alert("Model Output Uncertain"),
ET.NO_ENTRY: NoEntryAlert("Model Output Uncertain"), ET.NO_ENTRY: NoEntryAlert("Model Output Uncertain"),
}, },
# When the localizer detects an acceleration of more than 40 m/s^2 (~4G) we # When the localizer detects an acceleration of more than 40 m/s^2 (~4G) we
# alert the driver the device might have fallen from the windshield. # alert the driver the device might have fallen from the windshield.
EventName.deviceFalling: { EventName.deviceFalling: {
ET.SOFT_DISABLE: SoftDisableAlert("Device Fell Off Mount"), ET.SOFT_DISABLE: soft_disable_alert("Device Fell Off Mount"),
ET.NO_ENTRY: NoEntryAlert("Device Fell Off Mount"), ET.NO_ENTRY: NoEntryAlert("Device Fell Off Mount"),
}, },
EventName.lowMemory: { EventName.lowMemory: {
ET.SOFT_DISABLE: SoftDisableAlert("Low Memory: Reboot Your Device"), ET.SOFT_DISABLE: soft_disable_alert("Low Memory: Reboot Your Device"),
ET.PERMANENT: NormalPermanentAlert("Low Memory", "Reboot your Device"), ET.PERMANENT: NormalPermanentAlert("Low Memory", "Reboot your Device"),
ET.NO_ENTRY: NoEntryAlert("Low Memory: Reboot Your Device"), ET.NO_ENTRY: NoEntryAlert("Low Memory: Reboot Your Device"),
}, },
EventName.highCpuUsage: { EventName.highCpuUsage: {
#ET.SOFT_DISABLE: SoftDisableAlert("System Malfunction: Reboot Your Device"), #ET.SOFT_DISABLE: soft_disable_alert("System Malfunction: Reboot Your Device"),
#ET.PERMANENT: NormalPermanentAlert("System Malfunction", "Reboot your Device"), #ET.PERMANENT: NormalPermanentAlert("System Malfunction", "Reboot your Device"),
ET.NO_ENTRY: NoEntryAlert("System Malfunction: Reboot Your Device"), ET.NO_ENTRY: NoEntryAlert("System Malfunction: Reboot Your Device"),
}, },
@ -714,7 +737,7 @@ EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, boo
# Sometimes the USB stack on the device can get into a bad state # Sometimes the USB stack on the device can get into a bad state
# causing the connection to the panda to be lost # causing the connection to the panda to be lost
EventName.usbError: { EventName.usbError: {
ET.SOFT_DISABLE: SoftDisableAlert("USB Error: Reboot Your Device"), ET.SOFT_DISABLE: soft_disable_alert("USB Error: Reboot Your Device"),
ET.PERMANENT: NormalPermanentAlert("USB Error: Reboot Your Device", ""), ET.PERMANENT: NormalPermanentAlert("USB Error: Reboot Your Device", ""),
ET.NO_ENTRY: NoEntryAlert("USB Error: Reboot Your Device"), ET.NO_ENTRY: NoEntryAlert("USB Error: Reboot Your Device"),
}, },

@ -44,21 +44,23 @@ class LanePlanner:
self.path_offset = -PATH_OFFSET if wide_camera else PATH_OFFSET self.path_offset = -PATH_OFFSET if wide_camera else PATH_OFFSET
def parse_model(self, md): def parse_model(self, md):
if len(md.laneLines) == 4 and len(md.laneLines[0].t) == TRAJECTORY_SIZE: lane_lines = md.laneLines
self.ll_t = (np.array(md.laneLines[1].t) + np.array(md.laneLines[2].t))/2 if len(lane_lines) == 4 and len(lane_lines[0].t) == TRAJECTORY_SIZE:
self.ll_t = (np.array(lane_lines[1].t) + np.array(lane_lines[2].t))/2
# left and right ll x is the same # left and right ll x is the same
self.ll_x = md.laneLines[1].x self.ll_x = lane_lines[1].x
# only offset left and right lane lines; offsetting path does not make sense # only offset left and right lane lines; offsetting path does not make sense
self.lll_y = np.array(md.laneLines[1].y) - self.camera_offset self.lll_y = np.array(lane_lines[1].y) - self.camera_offset
self.rll_y = np.array(md.laneLines[2].y) - self.camera_offset self.rll_y = np.array(lane_lines[2].y) - self.camera_offset
self.lll_prob = md.laneLineProbs[1] self.lll_prob = md.laneLineProbs[1]
self.rll_prob = md.laneLineProbs[2] self.rll_prob = md.laneLineProbs[2]
self.lll_std = md.laneLineStds[1] self.lll_std = md.laneLineStds[1]
self.rll_std = md.laneLineStds[2] self.rll_std = md.laneLineStds[2]
if len(md.meta.desireState): desire_state = md.meta.desireState
self.l_lane_change_prob = md.meta.desireState[log.LateralPlan.Desire.laneChangeLeft] if len(desire_state):
self.r_lane_change_prob = md.meta.desireState[log.LateralPlan.Desire.laneChangeRight] self.l_lane_change_prob = desire_state[log.LateralPlan.Desire.laneChangeLeft]
self.r_lane_change_prob = desire_state[log.LateralPlan.Desire.laneChangeRight]
def get_d_path(self, v_ego, path_t, path_xyz): def get_d_path(self, v_ego, path_t, path_xyz):
# Reduce reliance on lanelines that are too far apart or # Reduce reliance on lanelines that are too far apart or

@ -10,7 +10,7 @@ class LatControlAngle():
def reset(self): def reset(self):
pass pass
def update(self, active, CS, CP, VM, params, desired_curvature, desired_curvature_rate): def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate):
angle_log = log.ControlsState.LateralAngleState.new_message() angle_log = log.ControlsState.LateralAngleState.new_message()
if CS.vEgo < 0.3 or not active: if CS.vEgo < 0.3 or not active:
@ -18,11 +18,11 @@ class LatControlAngle():
angle_steers_des = float(CS.steeringAngleDeg) angle_steers_des = float(CS.steeringAngleDeg)
else: else:
angle_log.active = True angle_log.active = True
angle_steers_des = math.degrees(VM.get_steer_from_curvature(-desired_curvature, CS.vEgo)) angle_steers_des = math.degrees(VM.get_steer_from_curvature(-desired_curvature, CS.vEgo, params.roll))
angle_steers_des += params.angleOffsetDeg angle_steers_des += params.angleOffsetDeg
angle_log.saturated = False angle_log.saturated = False
angle_log.steeringAngleDeg = float(CS.steeringAngleDeg) angle_log.steeringAngleDeg = float(CS.steeringAngleDeg)
angle_log.steeringAngleDesiredDeg = angle_steers_des angle_log.steeringAngleDesiredDeg = angle_steers_des
return 0, float(angle_steers_des), angle_log return 0, float(angle_steers_des), angle_log

@ -82,7 +82,7 @@ class LatControlINDI():
return self.sat_count > self.sat_limit return self.sat_count > self.sat_limit
def update(self, active, CS, CP, VM, params, curvature, curvature_rate): def update(self, active, CS, CP, VM, params, last_actuators, curvature, curvature_rate):
self.speed = CS.vEgo self.speed = CS.vEgo
# Update Kalman filter # Update Kalman filter
y = np.array([[math.radians(CS.steeringAngleDeg)], [math.radians(CS.steeringRateDeg)]]) y = np.array([[math.radians(CS.steeringAngleDeg)], [math.radians(CS.steeringRateDeg)]])
@ -93,11 +93,11 @@ class LatControlINDI():
indi_log.steeringRateDeg = math.degrees(self.x[1]) indi_log.steeringRateDeg = math.degrees(self.x[1])
indi_log.steeringAccelDeg = math.degrees(self.x[2]) indi_log.steeringAccelDeg = math.degrees(self.x[2])
steers_des = VM.get_steer_from_curvature(-curvature, CS.vEgo) steers_des = VM.get_steer_from_curvature(-curvature, CS.vEgo, params.roll)
steers_des += math.radians(params.angleOffsetDeg) steers_des += math.radians(params.angleOffsetDeg)
indi_log.steeringAngleDesiredDeg = math.degrees(steers_des) indi_log.steeringAngleDesiredDeg = math.degrees(steers_des)
rate_des = VM.get_steer_from_curvature(-curvature_rate, CS.vEgo) rate_des = VM.get_steer_from_curvature(-curvature_rate, CS.vEgo, 0)
indi_log.steeringRateDesiredDeg = math.degrees(rate_des) indi_log.steeringRateDesiredDeg = math.degrees(rate_des)
if CS.vEgo < 0.3 or not active: if CS.vEgo < 0.3 or not active:

@ -44,7 +44,7 @@ class LatControlLQR():
return self.sat_count > self.sat_limit return self.sat_count > self.sat_limit
def update(self, active, CS, CP, VM, params, desired_curvature, desired_curvature_rate): def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate):
lqr_log = log.ControlsState.LateralLQRState.new_message() lqr_log = log.ControlsState.LateralLQRState.new_message()
steers_max = get_steer_max(CP, CS.vEgo) steers_max = get_steer_max(CP, CS.vEgo)
@ -53,7 +53,7 @@ class LatControlLQR():
# Subtract offset. Zero angle should correspond to zero torque # Subtract offset. Zero angle should correspond to zero torque
steering_angle_no_offset = CS.steeringAngleDeg - params.angleOffsetAverageDeg steering_angle_no_offset = CS.steeringAngleDeg - params.angleOffsetAverageDeg
desired_angle = math.degrees(VM.get_steer_from_curvature(-desired_curvature, CS.vEgo)) desired_angle = math.degrees(VM.get_steer_from_curvature(-desired_curvature, CS.vEgo, params.roll))
instant_offset = params.angleOffsetDeg - params.angleOffsetAverageDeg instant_offset = params.angleOffsetDeg - params.angleOffsetAverageDeg
desired_angle += instant_offset # Only add offset that originates from vehicle model errors desired_angle += instant_offset # Only add offset that originates from vehicle model errors

@ -16,15 +16,15 @@ class LatControlPID():
def reset(self): def reset(self):
self.pid.reset() self.pid.reset()
def update(self, active, CS, CP, VM, params, desired_curvature, desired_curvature_rate): def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate):
pid_log = log.ControlsState.LateralPIDState.new_message() pid_log = log.ControlsState.LateralPIDState.new_message()
pid_log.steeringAngleDeg = float(CS.steeringAngleDeg) pid_log.steeringAngleDeg = float(CS.steeringAngleDeg)
pid_log.steeringRateDeg = float(CS.steeringRateDeg) pid_log.steeringRateDeg = float(CS.steeringRateDeg)
angle_steers_des_no_offset = math.degrees(VM.get_steer_from_curvature(-desired_curvature, CS.vEgo)) angle_steers_des_no_offset = math.degrees(VM.get_steer_from_curvature(-desired_curvature, CS.vEgo, params.roll))
angle_steers_des = angle_steers_des_no_offset + params.angleOffsetDeg angle_steers_des = angle_steers_des_no_offset + params.angleOffsetDeg
pid_log.steeringAngleDesiredDeg = angle_steers_des pid_log.steeringAngleDesiredDeg = angle_steers_des
pid_log.angleError = angle_steers_des - CS.steeringAngleDeg pid_log.angleError = angle_steers_des - CS.steeringAngleDeg
if CS.vEgo < 0.3 or not active: if CS.vEgo < 0.3 or not active:
output_steer = 0.0 output_steer = 0.0

@ -48,12 +48,13 @@ lenv.Clean(generated_files, Dir(gen))
lenv.Command(generated_files, lenv.Command(generated_files,
["lat_mpc.py"], ["lat_mpc.py"],
f"cd {Dir('.').abspath} && python lat_mpc.py") f"cd {Dir('.').abspath} && python3 lat_mpc.py")
lenv["CFLAGS"].append("-DACADOS_WITH_QPOASES") lenv["CFLAGS"].append("-DACADOS_WITH_QPOASES")
lenv["CXXFLAGS"].append("-DACADOS_WITH_QPOASES") lenv["CXXFLAGS"].append("-DACADOS_WITH_QPOASES")
lenv["CCFLAGS"].append("-Wno-unused") lenv["CCFLAGS"].append("-Wno-unused")
lenv["LINKFLAGS"].append("-Wl,--disable-new-dtags") if arch != "Darwin":
lenv["LINKFLAGS"].append("-Wl,--disable-new-dtags")
lib_solver = lenv.SharedLibrary(f"{gen}/acados_ocp_solver_lat", lib_solver = lenv.SharedLibrary(f"{gen}/acados_ocp_solver_lat",
build_files, build_files,
LIBS=['m', 'acados', 'hpipm', 'blasfeo', 'qpOASES_e']) LIBS=['m', 'acados', 'hpipm', 'blasfeo', 'qpOASES_e'])

@ -204,20 +204,22 @@ class LateralPlanner:
plan_solution_valid = self.solution_invalid_cnt < 2 plan_solution_valid = self.solution_invalid_cnt < 2
plan_send = messaging.new_message('lateralPlan') plan_send = messaging.new_message('lateralPlan')
plan_send.valid = sm.all_alive_and_valid(service_list=['carState', 'controlsState', 'modelV2']) plan_send.valid = sm.all_alive_and_valid(service_list=['carState', 'controlsState', 'modelV2'])
plan_send.lateralPlan.laneWidth = float(self.LP.lane_width)
plan_send.lateralPlan.dPathPoints = [float(x) for x in self.y_pts] lateralPlan = plan_send.lateralPlan
plan_send.lateralPlan.psis = [float(x) for x in self.lat_mpc.x_sol[0:CONTROL_N, 2]] lateralPlan.laneWidth = float(self.LP.lane_width)
plan_send.lateralPlan.curvatures = [float(x) for x in self.lat_mpc.x_sol[0:CONTROL_N, 3]] lateralPlan.dPathPoints = [float(x) for x in self.y_pts]
plan_send.lateralPlan.curvatureRates = [float(x) for x in self.lat_mpc.u_sol[0:CONTROL_N - 1]] + [0.0] lateralPlan.psis = [float(x) for x in self.lat_mpc.x_sol[0:CONTROL_N, 2]]
plan_send.lateralPlan.lProb = float(self.LP.lll_prob) lateralPlan.curvatures = [float(x) for x in self.lat_mpc.x_sol[0:CONTROL_N, 3]]
plan_send.lateralPlan.rProb = float(self.LP.rll_prob) lateralPlan.curvatureRates = [float(x) for x in self.lat_mpc.u_sol[0:CONTROL_N - 1]] + [0.0]
plan_send.lateralPlan.dProb = float(self.LP.d_prob) lateralPlan.lProb = float(self.LP.lll_prob)
lateralPlan.rProb = float(self.LP.rll_prob)
plan_send.lateralPlan.mpcSolutionValid = bool(plan_solution_valid) lateralPlan.dProb = float(self.LP.d_prob)
plan_send.lateralPlan.desire = self.desire lateralPlan.mpcSolutionValid = bool(plan_solution_valid)
plan_send.lateralPlan.useLaneLines = self.use_lanelines
plan_send.lateralPlan.laneChangeState = self.lane_change_state lateralPlan.desire = self.desire
plan_send.lateralPlan.laneChangeDirection = self.lane_change_direction lateralPlan.useLaneLines = self.use_lanelines
lateralPlan.laneChangeState = self.lane_change_state
lateralPlan.laneChangeDirection = self.lane_change_direction
pm.send('lateralPlan', plan_send) pm.send('lateralPlan', plan_send)

@ -7,21 +7,17 @@ from selfdrive.modeld.constants import T_IDXS
LongCtrlState = car.CarControl.Actuators.LongControlState LongCtrlState = car.CarControl.Actuators.LongControlState
STOPPING_TARGET_SPEED_OFFSET = 0.01
# As per ISO 15622:2018 for all speeds # As per ISO 15622:2018 for all speeds
ACCEL_MIN_ISO = -3.5 # m/s^2 ACCEL_MIN_ISO = -3.5 # m/s^2
ACCEL_MAX_ISO = 2.0 # m/s^2 ACCEL_MAX_ISO = 2.0 # m/s^2
def long_control_state_trans(CP, active, long_control_state, v_ego, v_target_future, v_pid, def long_control_state_trans(CP, active, long_control_state, v_ego, v_target_future,
output_accel, brake_pressed, cruise_standstill, min_speed_can): output_accel, brake_pressed, cruise_standstill):
"""Update longitudinal control state machine""" """Update longitudinal control state machine"""
stopping_target_speed = min_speed_can + STOPPING_TARGET_SPEED_OFFSET
stopping_condition = (v_ego < 2.0 and cruise_standstill) or \ stopping_condition = (v_ego < 2.0 and cruise_standstill) or \
(v_ego < CP.vEgoStopping and (v_ego < CP.vEgoStopping and
((v_pid < stopping_target_speed and v_target_future < stopping_target_speed) or (v_target_future < CP.vEgoStopping or brake_pressed))
brake_pressed))
starting_condition = v_target_future > CP.vEgoStarting and not cruise_standstill starting_condition = v_target_future > CP.vEgoStarting and not cruise_standstill
@ -69,15 +65,16 @@ class LongControl():
"""Update longitudinal control. This updates the state machine and runs a PID loop""" """Update longitudinal control. This updates the state machine and runs a PID loop"""
# Interp control trajectory # Interp control trajectory
# TODO estimate car specific lag, use .15s for now # TODO estimate car specific lag, use .15s for now
if len(long_plan.speeds) == CONTROL_N: speeds = long_plan.speeds
v_target_lower = interp(CP.longitudinalActuatorDelayLowerBound, T_IDXS[:CONTROL_N], long_plan.speeds) if len(speeds) == CONTROL_N:
a_target_lower = 2 * (v_target_lower - long_plan.speeds[0])/CP.longitudinalActuatorDelayLowerBound - long_plan.accels[0] v_target_lower = interp(CP.longitudinalActuatorDelayLowerBound, T_IDXS[:CONTROL_N], speeds)
a_target_lower = 2 * (v_target_lower - speeds[0])/CP.longitudinalActuatorDelayLowerBound - long_plan.accels[0]
v_target_upper = interp(CP.longitudinalActuatorDelayUpperBound, T_IDXS[:CONTROL_N], long_plan.speeds) v_target_upper = interp(CP.longitudinalActuatorDelayUpperBound, T_IDXS[:CONTROL_N], speeds)
a_target_upper = 2 * (v_target_upper - long_plan.speeds[0])/CP.longitudinalActuatorDelayUpperBound - long_plan.accels[0] a_target_upper = 2 * (v_target_upper - speeds[0])/CP.longitudinalActuatorDelayUpperBound - long_plan.accels[0]
a_target = min(a_target_lower, a_target_upper) a_target = min(a_target_lower, a_target_upper)
v_target_future = long_plan.speeds[-1] v_target_future = speeds[-1]
else: else:
v_target_future = 0.0 v_target_future = 0.0
a_target = 0.0 a_target = 0.0
@ -91,8 +88,8 @@ class LongControl():
# Update state machine # Update state machine
output_accel = self.last_output_accel output_accel = self.last_output_accel
self.long_control_state = long_control_state_trans(CP, active, self.long_control_state, CS.vEgo, self.long_control_state = long_control_state_trans(CP, active, self.long_control_state, CS.vEgo,
v_target_future, self.v_pid, output_accel, v_target_future, output_accel,
CS.brakePressed, CS.cruiseState.standstill, CP.minSpeedCan) CS.brakePressed, CS.cruiseState.standstill)
if self.long_control_state == LongCtrlState.off or CS.gasPressed: if self.long_control_state == LongCtrlState.off or CS.gasPressed:
self.reset(CS.vEgo) self.reset(CS.vEgo)
@ -100,7 +97,7 @@ class LongControl():
# tracking objects and driving # tracking objects and driving
elif self.long_control_state == LongCtrlState.pid: elif self.long_control_state == LongCtrlState.pid:
self.v_pid = long_plan.speeds[0] self.v_pid = speeds[0]
# Toyota starts braking more when it thinks you want to stop # Toyota starts braking more when it thinks you want to stop
# Freeze the integrator so we don't accelerate to compensate, and don't allow positive acceleration # Freeze the integrator so we don't accelerate to compensate, and don't allow positive acceleration
@ -119,7 +116,6 @@ class LongControl():
if not CS.standstill or output_accel > CP.stopAccel: if not CS.standstill or output_accel > CP.stopAccel:
output_accel -= CP.stoppingDecelRate * DT_CTRL output_accel -= CP.stoppingDecelRate * DT_CTRL
output_accel = clip(output_accel, accel_limits[0], accel_limits[1]) output_accel = clip(output_accel, accel_limits[0], accel_limits[1])
self.reset(CS.vEgo) self.reset(CS.vEgo)
# Intention is to move again, release brake fast before handing control to PID # Intention is to move again, release brake fast before handing control to PID

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

Loading…
Cancel
Save