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
path: selfdrive/test/longitudinal_maneuvers/out/longitudinal/
test_car_models:
name: car models
test_cars:
name: cars
runs-on: ubuntu-20.04
timeout-minutes: 50
strategy:
fail-fast: false
matrix:
job: [0, 1, 2, 3]
steps:
@ -385,7 +386,7 @@ jobs:
- name: Test car models
run: |
${{ 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"
env:
NUM_JOBS: 4

@ -45,7 +45,20 @@ jobs:
- uses: actions/checkout@v2
with:
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
run: |
eval "$BUILD"

1
.gitignore vendored

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

@ -13,14 +13,14 @@ repos:
rev: v0.910-1
hooks:
- 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',
'types-pycurl']
- repo: https://github.com/PyCQA/flake8
rev: 4.0.1
hooks:
- 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:
- --select=F,E112,E113,E304,E502,E701,E702,E703,E71,E72,E731,W191,W6
- --statistics
@ -31,7 +31,7 @@ repos:
entry: pylint
language: system
types: [python]
exclude: '^(pyextra)|(cereal)|(rednose)|(panda)|(laika)|(laika_repo)|(rednose_repo)/'
exclude: '^(pyextra/)|(cereal/)|(rednose/)|(panda/)|(laika/)|(laika_repo/)|(rednose_repo/)/'
- repo: local
hooks:
- id: cppcheck
@ -39,7 +39,7 @@ repos:
entry: cppcheck
language: system
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:
- --error-exitcode=1
- --language=c++

@ -1,70 +1,26 @@
FROM ubuntu:20.04
ENV PYTHONUNBUFFERED 1
ENV DEBIAN_FRONTEND=noninteractive
RUN apt-get update && apt-get install -y --no-install-recommends \
autoconf \
build-essential \
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 apt-get update && \
apt-get install -y --no-install-recommends sudo tzdata locales && \
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
ENV LANG en_US.UTF-8
ENV LANGUAGE en_US:en
ENV LC_ALL en_US.UTF-8
RUN curl -L https://github.com/pyenv/pyenv-installer/raw/master/bin/pyenv-installer | bash
ENV PATH="/root/.pyenv/bin:/root/.pyenv/shims:${PATH}"
COPY Pipfile Pipfile.lock /tmp/
RUN pyenv install 3.8.10 && \
pyenv global 3.8.10 && \
pyenv rehash && \
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 && \
ENV PIPENV_SYSTEM=1
COPY Pipfile Pipfile.lock .python-version update_requirements.sh /tmp/
COPY tools/ubuntu_setup.sh /tmp/tools/
RUN cd /tmp && \
tools/ubuntu_setup.sh && \
rm -rf /tmp/* && \
rm -rf /var/lib/apt/lists/* && \
pip uninstall -y pipenv
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 = "*"
pygame = "*"
pyprof2calltree = "*"
pytest = "*"
pytest-xdist = "*"
reverse_geocoder = "*"
scipy = "*"
sphinx = "*"
sphinx-sitemap = "*"
sphinx-rtd-theme = "*"
breathe = "*"
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)
========================
* New driving model
* 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
* Better longitudinal policy, especially in low speed traffic
* New alert sounds

@ -127,6 +127,7 @@ else:
"/opt/homebrew/lib",
"/usr/local/opt/openssl/lib",
"/opt/homebrew/opt/openssl/lib",
f"#third_party/acados/{arch}/lib",
"/System/Library/Frameworks/OpenGL.framework/Libraries",
]
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:
mod = __import__(cache)
except Exception:
print("cache miss {0}".format(cache))
print(f"cache miss {cache}")
compile_code(cache, c_code, c_header, tmpdir, cflags, libraries)
mod = __import__(cache)
finally:

@ -35,7 +35,7 @@ def get_tmpdir_on_same_filesystem(path):
if len(parts) > 1 and parts[1] == "scratch":
return "/scratch/tmp"
elif len(parts) > 2 and parts[2] == "runner":
return "/{}/runner/tmp".format(parts[1])
return f"/{parts[1]}/runner/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))
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("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:
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:
@ -79,7 +79,7 @@ class Ratekeeper:
remaining = self._next_frame_time - sec_since_boot()
self._next_frame_time += self._interval
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
self._frame += 1
self._remaining = remaining

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

@ -8,7 +8,7 @@ from common.file_helpers import atomic_write_in_dir
class TestFileHelpers(unittest.TestCase):
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:
f.write("test")

@ -12,7 +12,7 @@ class Timeout:
"""
def __init__(self, seconds, error_msg=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.error_msg = error_msg

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

@ -1,46 +1,48 @@
# 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/)
* Join our [Discord](https://discord.comma.ai)
* Make sure you have a [GitHub account](https://github.com/signup/free)
* 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.
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:
```
git clone https://github.com/commaai/openpilot.git --recursive
```
Or alternatively, when on the master branch:
```
git submodule update --init
```
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
### Automated Testing
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.
### Code Style and Linting
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`.

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
from os.path import exists
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('..'))
OPENPILOT_ROOT = os.path.abspath(r'../../') # from openpilot/build/docs
VERSION = get_version()
# -- Project information -----------------------------------------------------
project = 'openpilot'
project = 'openpilot docs'
copyright = '2021, comma.ai'
author = 'comma.ai'
version = VERSION
release = VERSION
language = 'en'
# -- General configuration ---------------------------------------------------
@ -39,8 +45,34 @@ extensions = [
'sphinx_rtd_theme', # Read The Docs theme
'myst_parser', # Markdown parsing
'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.
templates_path = ['_templates']
@ -54,43 +86,38 @@ exclude_patterns = []
# Breathe Configuration
# 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_default_members = ('members', 'private-members', 'undoc-members')
breathe_domain_by_extension = {
"h" : "cc",
}
"h": "cc",
}
breathe_implementation_filename_extensions = ['.c', '.cc', '.cpp']
breathe_doxygen_config_options = {}
breathe_projects_source = {
# "loggerd" : ("../../../selfdrive/loggerd", ["logger.h"])
}
breathe_projects_source = {}
# only document files that have accompanying .cc files next to them
print("searching for c_docs...")
for root, dirs, files in os.walk(OPENPILOT_ROOT):
found = False
breath_src = {}
breathe_srcs_list = []
for file in files:
ccFile = os.path.join(root, file)[:-2] +".cc"
for root, dirs, files in os.walk(BASEDIR):
found = False
breath_src = {}
breathe_srcs_list = []
if file.endswith(".h") and exists(ccFile):
f = os.path.join(root, file)
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})")
for file in files:
ccFile = os.path.join(root, file)[:-2] + ".cc"
breathe_srcs_list.append(file)
# breathe_srcs_list.append(ccFile)
found = True
if file.endswith(".h") and exists(ccFile):
f = os.path.join(root, file)
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:
breath_src[parent_dir] = (parent_dir_abs, breathe_srcs_list)
breathe_projects_source.update(breath_src)
if found:
breath_src[parent_dir] = (parent_dir_abs, breathe_srcs_list)
breathe_projects_source.update(breath_src)
print(f"breathe_projects_source: {breathe_projects_source.keys()}")
# input("Press Enter to continue...")
@ -101,8 +128,18 @@ print(f"breathe_projects_source: {breathe_projects_source.keys()}")
# a list of builtin themes.
#
html_theme = 'sphinx_rtd_theme'
html_show_copyright = True
# 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,
# so a file named "default.css" will overwrite the builtin "default.css".
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')
HANDLER_THREADS = int(os.getenv('HANDLER_THREADS', "4"))
LOCAL_PORT_WHITELIST = set([8022])
LOCAL_PORT_WHITELIST = {8022}
LOG_ATTR_NAME = 'user.upload'
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 = {}
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):
end_event = threading.Event()
@ -111,6 +133,7 @@ def upload_handler(end_event):
try:
cur_upload_items[tid] = upload_queue.get(timeout=1)._replace(current=True)
if cur_upload_items[tid].id in cancelled_uploads:
cancelled_uploads.remove(cur_upload_items[tid].id)
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)
_do_upload(cur_upload_items[tid], cb)
UploadQueueCache.cache(upload_queue)
except (requests.exceptions.Timeout, requests.exceptions.ConnectionError, requests.exceptions.SSLError) as e:
cloudlog.warning(f"athena.upload_handler.retry {e} {cur_upload_items[tid]}")
@ -131,6 +155,8 @@ def upload_handler(end_event):
current=False
)
upload_queue.put_nowait(item)
UploadQueueCache.cache(upload_queue)
cur_upload_items[tid] = None
for _ in range(RETRY_DELAY):
@ -248,6 +274,7 @@ def uploadFileToUrl(fn, url, headers):
item = item._replace(id=upload_id)
upload_queue.put_nowait(item)
UploadQueueCache.cache(upload_queue)
return {"enqueued": 1, "item": item._asdict()}
@ -260,7 +287,7 @@ def listUploadQueue():
@dispatcher.add_method
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:
return 404
@ -280,8 +307,7 @@ def startLocalProxy(global_end_event, remote_ws_uri, local_port):
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()
ws = create_connection(remote_ws_uri,
cookie="jwt=" + identity_token,
@ -312,7 +338,7 @@ def getPublicKey():
if not os.path.isfile(PERSIST + '/comma/id_rsa.pub'):
return None
with open(PERSIST + '/comma/id_rsa.pub', 'r') as f:
with open(PERSIST + '/comma/id_rsa.pub') as f:
return f.read()
@ -393,7 +419,7 @@ def log_handler(end_event):
curr_time = int(time.time())
log_path = os.path.join(SWAGLOG_DIR, log_entry)
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 = {
"method": "forwardLogs",
"params": {
@ -525,6 +551,7 @@ def backoff(retries):
def main():
params = Params()
dongle_id = params.get("DongleId", encoding='utf-8')
UploadQueueCache.initialize(upload_queue)
ws_uri = ATHENA_HOST + "/ws/v2/" + dongle_id
api = Api(dongle_id)

@ -6,7 +6,7 @@ from multiprocessing import Process
from common.params import Params
from selfdrive.manager.process import launcher
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"
@ -14,12 +14,12 @@ ATHENA_MGR_PID_PARAM = "AthenadPid"
def main():
params = Params()
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:
while 1:
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.join()
cloudlog.event("athenad exited", exitcode=proc.exitcode)

@ -50,18 +50,28 @@ class MockApi():
class MockParams():
def __init__(self):
self.params = {
"DongleId": b"0000000000000000",
"GithubSshKeys": b"ssh-rsa AAAAB3NzaC1yc2EAAAADAQABAAABAQC307aE+nuHzTAgaJhzSf5v7ZZQW9gaperjhCmyPyl4PzY7T1mDGenTlVTN7yoVFZ9UfO9oMQqo0n1OwDIiqbIFxqnhrHU0cYfj88rI85m5BEKlNu5RdaVTj1tcbaPpQc5kZEolaI1nDDjzV0lwS7jo5VYDHseiJHlik3HH1SgtdtsuamGR2T80q1SyW+5rHoMOJG73IH2553NnWuikKiuikGHUYBd00K1ilVAK2xSiMWJp55tQfZ0ecr9QjEsJ+J/efL4HqGNXhffxvypCXvbUYAFSddOwXUPo5BTKevpxMtH+2YrkpSjocWA04VnTYFiPG6U4ItKmbLOTFZtPzoez private" # noqa: E501
}
default_params = {
"DongleId": b"0000000000000000",
"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):
ret = self.params.get(k)
ret = MockParams.params.get(k)
if ret is not None and encoding is not None:
ret = ret.decode(encoding)
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():
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 cereal import messaging
class TestAthenadMethods(unittest.TestCase):
@classmethod
def setUpClass(cls):
cls.SOCKET_PORT = 45454
athenad.Params = MockParams
athenad.ROOT = tempfile.mkdtemp()
athenad.SWAGLOG_DIR = swaglog.SWAGLOG_DIR = tempfile.mkdtemp()
athenad.Params = MockParams
athenad.Api = MockApi
athenad.LOCAL_PORT_WHITELIST = set([cls.SOCKET_PORT])
athenad.LOCAL_PORT_WHITELIST = {cls.SOCKET_PORT}
def setUp(self):
MockParams.restore_defaults()
athenad.upload_queue = queue.Queue()
athenad.cur_upload_items.clear()
athenad.cancelled_uploads.clear()
for i in os.listdir(athenad.ROOT):
p = os.path.join(athenad.ROOT, i)
@ -249,6 +252,26 @@ class TestAthenadMethods(unittest.TestCase):
items = dispatcher["listUploadQueue"]()
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')
def test_startLocalProxy(self, mock_create_connection):
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) {
set_thread_name("boardd_can_send");
util::set_thread_name("boardd_can_send");
AlignedBuffer aligned_buf;
Context * context = Context::create();
SubSocket * subscriber = SubSocket::create(context, "sendcan");
std::unique_ptr<Context> context(Context::create());
std::unique_ptr<SubSocket> subscriber(SubSocket::create(context.get(), "sendcan"));
assert(subscriber != NULL);
subscriber->setTimeout(100);
// run as fast as messages come in
while (!do_exit && check_all_connected(pandas)) {
Message * msg = subscriber->receive();
std::unique_ptr<Message> msg(subscriber->receive());
if (!msg) {
if (errno == EINTR) {
do_exit = true;
@ -223,27 +223,20 @@ void can_send_thread(std::vector<Panda *> pandas, bool fake_send) {
continue;
}
capnp::FlatArrayMessageReader cmsg(aligned_buf.align(msg));
capnp::FlatArrayMessageReader cmsg(aligned_buf.align(msg.get()));
cereal::Event::Reader event = cmsg.getRoot<cereal::Event>();
//Dont send if older than 1 second
if (nanos_since_boot() - event.getLogMonoTime() < 1e9) {
if (!fake_send) {
for (const auto& panda : pandas) {
panda->can_send(event.getSendcan());
}
if ((nanos_since_boot() - event.getLogMonoTime() < 1e9) && !fake_send) {
for (const auto& panda : pandas) {
panda->can_send(event.getSendcan());
}
}
delete msg;
}
delete subscriber;
delete context;
}
void can_recv_thread(std::vector<Panda *> pandas) {
set_thread_name("boardd_can_recv");
util::set_thread_name("boardd_can_recv");
// can = 8006
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) {
set_thread_name("boardd_panda_state");
util::set_thread_name("boardd_panda_state");
Params params;
SubMaster sm({"controlsState"});
Panda *peripheral_panda = pandas[0];
bool ignition_last = false;
std::future<bool> safety_future;
@ -452,8 +447,11 @@ void panda_state_thread(PubMaster *pm, std::vector<Panda *> pandas, bool spoofin
ignition_last = ignition;
sm.update(0);
const bool engaged = sm.allAliveAndValid({"controlsState"}) && sm["controlsState"].getControlsState().getEnabled();
for (const auto &panda : pandas) {
panda->send_heartbeat();
panda->send_heartbeat(engaged);
}
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) {
set_thread_name("boardd_peripheral_control");
util::set_thread_name("boardd_peripheral_control");
SubMaster sm({"deviceState", "driverCameraState"});
@ -547,12 +545,12 @@ static void pigeon_publish_raw(PubMaster &pm, const std::string &dat) {
}
void pigeon_thread(Panda *panda) {
set_thread_name("boardd_pigeon");
util::set_thread_name("boardd_pigeon");
PubMaster pm({"ubloxRaw"});
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, int64_t> cls_max_dt = {
@ -620,8 +618,6 @@ void pigeon_thread(Panda *panda) {
// 10ms - 100 Hz
util::sleep_for(10);
}
delete pigeon;
}
int main(int argc, char *argv[]) {
@ -629,9 +625,9 @@ int main(int argc, char *argv[]) {
if (!Hardware::PC()) {
int err;
err = set_realtime_priority(54);
err = util::set_realtime_priority(54);
assert(err == 0);
err = set_core_affinity({Hardware::TICI() ? 4 : 3});
err = util::set_core_affinity({Hardware::TICI() ? 4 : 3});
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);
}
void Panda::send_heartbeat() {
usb_write(0xf3, 1, 0);
void Panda::send_heartbeat(bool engaged) {
usb_write(0xf3, engaged, 0);
}
void Panda::set_can_speed_kbps(uint16_t bus, uint16_t speed) {

@ -108,7 +108,7 @@ class Panda {
std::optional<std::string> get_serial();
void set_power_saving(bool power_saving);
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_data_speed_kbps(uint16_t bus, uint16_t speed);
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)):
bus = random.choice([b for b in range(3*num_pandas) if b % 4 != 3])
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))
to_send.append(make_can_msg(addr, dat, bus))
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.setTargetGreyFraction(frame_data.target_grey_fraction);
framed.setLensPos(frame_data.lens_pos);
framed.setLensSag(frame_data.lens_sag);
framed.setLensErr(frame_data.lens_err);
framed.setLensTruePos(frame_data.lens_true_pos);
}
@ -351,7 +350,7 @@ void *processing_thread(MultiCameraState *cameras, CameraState *cs, process_thre
} else {
thread_name = "WideRoadCamera";
}
set_thread_name(thread_name);
util::set_thread_name(thread_name);
uint32_t cnt = 0;
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));
}
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;
if (cnt % j == 0) {
sm->update(0);
driver_cam_auto_exposure(c, *sm);
s->sm->update(0);
driver_cam_auto_exposure(c, *(s->sm));
}
MessageBuilder msg;
auto framed = msg.initEvent().initDriverCameraState();
@ -421,5 +420,5 @@ void common_process_driver_camera(SubMaster *sm, PubMaster *pm, CameraState *c,
if (env_send_driver) {
framed.setImage(get_frame_image(&c->buf));
}
pm->send("driverCameraState", msg);
s->pm->send("driverCameraState", msg);
}

@ -68,7 +68,6 @@ typedef struct FrameMetadata {
// Focus
unsigned int lens_pos;
float lens_sag;
float lens_err;
float lens_true_pos;
} 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);
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);
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_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);
}
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) {
@ -852,21 +852,7 @@ static void parse_autofocus(CameraState *s, uint8_t *d) {
s->focus_err = max_focus*1.0;
}
static std::optional<float> get_accel_z(SubMaster *sm) {
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) {
static void do_autofocus(CameraState *s) {
float lens_true_pos = s->lens_true_pos.load();
if (!isnan(s->focus_err)) {
// learn lens_true_pos
@ -874,23 +860,10 @@ static void do_autofocus(CameraState *s, SubMaster *sm) {
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
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);
/*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);
actuator_move(s, lens_true_pos);
}
void camera_autoexposure(CameraState *s, float grey_frac) {
@ -1045,13 +1018,12 @@ static void ops_thread(MultiCameraState *s) {
CameraExpInfo road_cam_op;
CameraExpInfo driver_cam_op;
set_thread_name("camera_settings");
SubMaster sm({"sensorEvents"});
util::set_thread_name("camera_settings");
while(!do_exit) {
road_cam_op = road_cam_exp.load();
if (road_cam_op.op_id != last_road_cam_op_id) {
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;
}
@ -1086,10 +1058,6 @@ static void setup_self_recover(CameraState *c, const uint16_t *lapres, size_t la
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
void process_road_camera(MultiCameraState *s, CameraState *c, int cnt) {
const CameraBuf *b = &c->buf;
@ -1121,7 +1089,7 @@ void cameras_run(MultiCameraState *s) {
std::vector<std::thread> threads;
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->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};
@ -1169,7 +1137,6 @@ void cameras_run(MultiCameraState *s) {
.frame_length = (uint32_t)c->frame_length,
.integ_lines = (uint32_t)c->cur_integ_lines,
.lens_pos = c->cur_lens_pos,
.lens_sag = c->last_sag_acc_z,
.lens_err = c->focus_err,
.lens_true_pos = c->lens_true_pos,
.gain = c->cur_gain_frac,

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

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

@ -67,12 +67,12 @@ void run_camera(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);
}
// void driver_camera_thread(CameraState *s) {
// set_thread_name("replay_driver_camera_thread");
// util::set_thread_name("replay_driver_camera_thread");
// 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) {
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
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));
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);
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);
}
LapConv::LapConv(cl_device_id device_id, cl_context ctx, int rgb_width, int rgb_height, int filter_size)
: width(rgb_width / NUM_SEGMENTS_X), height(rgb_height / NUM_SEGMENTS_Y),
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), rgb_stride(rgb_stride),
roi_buf(width * height * 3), result_buf(width * height) {
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 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) {
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));

@ -16,16 +16,11 @@
#define LM_THRESH 120
#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
class LapConv {
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();
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_kernel krnl;
const int width, height;
const int rgb_stride;
std::vector<uint8_t> roi_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[]) {
if (!Hardware::PC()) {
int ret;
ret = set_realtime_priority(53);
ret = util::set_realtime_priority(53);
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
}

@ -54,7 +54,7 @@ class TestCamerad(unittest.TestCase):
self.assertTrue(abs(dfid - 1) <= SKIP_FRAME_TOLERANCE, "%s frame id diff is %d" % (camera, dfid))
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_ts[camera] = ct

@ -1,7 +1,7 @@
import os
from common.params import Params
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.vin import get_vin, VIN_UNKNOWN
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):
if get_comma_remote() and get_tested_branch():
if is_comma_remote() and is_tested_branch():
event = EventName.startup
else:
event = EventName.startupMaster
@ -39,7 +39,7 @@ def get_one_can(logcan):
def load_interfaces(brand_names):
ret = {}
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
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')]:
try:
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("__")]
brand_names[brand_name] = model_names
except (ImportError, IOError):
except (ImportError, OSError):
pass
return brand_names

@ -1,3 +1,4 @@
from cereal import car
from selfdrive.car import apply_toyota_steer_torque_limits
from selfdrive.car.chrysler.chryslercan import create_lkas_hud, create_lkas_command, \
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
frame = CS.lkas_counter
if self.prev_frame == frame:
return []
return car.CarControl.Actuators.new_message(), []
# *** compute control surfaces ***
# steer torque
new_steer = int(round(actuators.steer * CarControllerParams.STEER_MAX))
apply_steer = apply_toyota_steer_torque_limits(new_steer, self.apply_steer_last,
@ -67,4 +67,7 @@ class CarController():
self.ccframe += 1
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):
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 can_sends
return self.CC.update(c.enabled, self.CS, c.actuators, c.cruiseControl.cancel, c.hudControl.visualAlert)

@ -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')]:
try:
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):
attr_values = getattr(values, attr)
else:
@ -28,7 +28,7 @@ def get_attr_from_cars(attr, result=dict, combine_brands=True):
elif isinstance(attr_values, list):
result += attr_values
except (ImportError, IOError):
except (ImportError, OSError):
pass
return result

@ -32,7 +32,7 @@ class CarController():
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
if TOGGLE_DEBUG:
@ -83,4 +83,4 @@ class CarController():
self.main_on_last = CS.out.cruiseState.available
self.steer_alert_last = steer_alert
return can_sends
return actuators, can_sends

@ -63,8 +63,8 @@ class CarInterface(CarInterfaceBase):
# to be called @ 100hz
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)
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
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
REQUESTS = [
# Subaru
(
"subaru",
[TESTER_PRESENT_REQUEST, SUBARU_VERSION_REQUEST],
[TESTER_PRESENT_RESPONSE, SUBARU_VERSION_RESPONSE],
DEFAULT_RX_OFFSET,
),
# Hyundai
(
"hyundai",
@ -221,7 +234,7 @@ def match_fw_to_car_fuzzy(fw_versions_dict, log=True, exclude=None):
if match_count >= 2:
if log:
cloudlog.error(f"Fingerprinted {candidate} using fuzzy match. {match_count} matching ECUs")
return set([candidate])
return {candidate}
else:
return set()
@ -362,7 +375,7 @@ if __name__ == "__main__":
print("Getting vin...")
addr, vin = get_vin(logcan, sendcan, 1, retry=10, debug=args.debug)
print(f"VIN: {vin}")
print("Getting VIN took %.3f s" % (time.time() - t))
print(f"Getting VIN took {time.time() - t:.3f} s")
print()
t = time.time()
@ -379,4 +392,4 @@ if __name__ == "__main__":
print()
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):
self.start_time = 0.
self.apply_steer_last = 0
self.apply_gas = 0
self.apply_brake = 0
self.lka_steering_cmd_counter_last = -1
self.lka_icon_status_last = (False, 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))
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
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
at_full_stop = enabled and CS.out.standstill
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_gas_regen_command(self.packer_pt, CanBus.POWERTRAIN, apply_gas, idx, enabled, 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, self.apply_gas, idx, enabled, at_full_stop))
# Send dashboard UI commands (ACC status), 25hz
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))
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:
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.steeringAngleDeg = pt_cp.vl["PSCMSteeringAngle"]["SteeringWheelAngle"]
@ -90,7 +90,7 @@ class CarState(CarStateBase):
("LeftSeatBelt", "BCMDoorBeltStatus", 0),
("RightSeatBelt", "BCMDoorBeltStatus", 0),
("TurnSignals", "BCMTurnSignals", 0),
("AcceleratorPedal", "AcceleratorPedal", 0),
("AcceleratorPedal2", "AcceleratorPedal2", 0),
("CruiseState", "AcceleratorPedal2", 0),
("ACCButtons", "ASCMSteeringButton", CruiseButtons.UNPRESS),
("SteeringWheelAngle", "PSCMSteeringAngle", 0),
@ -117,7 +117,6 @@ class CarState(CarStateBase):
("EPBStatus", 20),
("EBCMWheelSpdFront", 20),
("EBCMWheelSpdRear", 20),
("AcceleratorPedal", 33),
("AcceleratorPedal2", 33),
("ASCMSteeringButton", 33),
("ECMEngineStatus", 100),

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

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

@ -105,6 +105,11 @@ class CarController():
self.last_pump_ts = 0.
self.packer = CANPacker(dbc_name)
self.accel = 0
self.speed = 0
self.gas = 0
self.brake = 0
self.params = CarControllerParams(CP)
def update(self, enabled, active, CS, frame, actuators, pcm_cancel_cmd,
@ -211,10 +216,9 @@ class CarController():
ts = frame * DT_CTRL
if CS.CP.carFingerprint in HONDA_BOSCH:
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)
can_sends.extend(hondacan.create_acc_commands(self.packer, enabled, active, accel, bosch_gas, idx, stopping, starting, CS.CP.carFingerprint))
self.accel = clip(accel, P.BOSCH_ACCEL_MIN, P.BOSCH_ACCEL_MAX)
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, self.gas, idx, stopping, starting, CS.CP.carFingerprint))
else:
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))
@ -224,6 +228,7 @@ class CarController():
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))
self.apply_brake_last = apply_brake
self.brake = apply_brake / P.NIDEC_BRAKE_MAX
if CS.CP.enableGasInterceptor:
# 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
# when you do enable.
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:
apply_gas = 0.0
can_sends.append(create_gas_interceptor_command(self.packer, apply_gas, idx))
hud = HUDData(int(pcm_accel), int(round(hud_v_cruise)), hud_car,
hud_lanes, fcw_display, acc_alert, steer_required)
self.gas = 0.0
can_sends.append(create_gas_interceptor_command(self.packer, self.gas, idx))
# Send dashboard UI commands.
if (frame % 10) == 0:
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))
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.steerRatio = 11.95 # as spec
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
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
else:
raise ValueError("unsupported car %s" % candidate)
raise ValueError(f"unsupported car {candidate}")
# These cars use alternate user brake msg (0x1BE)
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.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 = []
@ -433,18 +432,19 @@ class CarInterface(CarInterfaceBase):
# pass in a car.CarControl
# to be called @ 100hz
def apply(self, c):
if c.hudControl.speedVisible:
hud_v_cruise = c.hudControl.setSpeed * CV.MS_TO_KPH
hud_control = c.hudControl
if hud_control.speedVisible:
hud_v_cruise = hud_control.setSpeed * CV.MS_TO_KPH
else:
hud_v_cruise = 255
can_sends = self.CC.update(c.enabled, c.active, self.CS, self.frame,
c.actuators,
c.cruiseControl.cancel,
hud_v_cruise,
c.hudControl.lanesVisible,
hud_show_car=c.hudControl.leadVisible,
hud_alert=c.hudControl.visualAlert)
ret = self.CC.update(c.enabled, c.active, self.CS, self.frame,
c.actuators,
c.cruiseControl.cancel,
hud_v_cruise,
hud_control.lanesVisible,
hud_show_car=hud_control.leadVisible,
hud_alert=hud_control.visualAlert)
self.frame += 1
return can_sends
return ret

@ -1385,9 +1385,9 @@ STEER_THRESHOLD = {
CAR.CRV_EU: 400,
}
HONDA_NIDEC_ALT_PCM_ACCEL = set([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,
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,
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_NIDEC_ALT_PCM_ACCEL = {CAR.ODYSSEY}
HONDA_NIDEC_ALT_SCM_MESSAGES = {CAR.ACURA_ILX, CAR.ACURA_RDX, CAR.CRV, CAR.CRV_EU, CAR.FIT, CAR.FREED, CAR.HRV, CAR.ODYSSEY_CHN,
CAR.PILOT, CAR.PILOT_2019, CAR.PASSPORT, CAR.RIDGELINE}
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}
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.steer_rate_limited = False
self.last_resume_frame = 0
self.accel = 0
def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, visual_alert, hud_speed,
left_lane, right_lane, left_lane_depart, right_lane_depart):
@ -100,12 +101,13 @@ class CarController():
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)
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
if frame % 5 == 0 and self.car_fingerprint in [CAR.SONATA, CAR.PALISADE, CAR.IONIQ, CAR.KIA_NIRO_EV, CAR.KIA_NIRO_HEV_2021,
CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV, CAR.KIA_CEED, CAR.KIA_SELTOS, CAR.KONA_EV,
CAR.ELANTRA_2021, CAR.ELANTRA_HEV_2021, CAR.SONATA_HYBRID, CAR.KONA_HEV, CAR.SANTA_FE_2022,
CAR.KIA_K5_2021, CAR.IONIQ_HEV_2022, CAR.SANTA_FE_HEV_2022, CAR.GENESIS_G70_2020]:
CAR.KIA_K5_2021, CAR.IONIQ_HEV_2022, CAR.SANTA_FE_HEV_2022, CAR.GENESIS_G70_2020, CAR.SANTA_FE_PHEV_2022]:
can_sends.append(create_lfahda_mfc(self.packer, enabled))
# 5 Hz ACC options
@ -116,4 +118,8 @@ class CarController():
if frame % 50 == 0 and CS.CP.openpilotLongitudinalControl:
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,
CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV, CAR.KIA_SELTOS, CAR.ELANTRA_2021, CAR.GENESIS_G70_2020,
CAR.ELANTRA_HEV_2021, CAR.SONATA_HYBRID, CAR.KONA_EV, CAR.KONA_HEV, CAR.SANTA_FE_2022,
CAR.KIA_K5_2021, CAR.IONIQ_HEV_2022, CAR.SANTA_FE_HEV_2022]:
CAR.KIA_K5_2021, CAR.IONIQ_HEV_2022, CAR.SANTA_FE_HEV_2022, CAR.SANTA_FE_PHEV_2022]:
values["CF_Lkas_LdwsActivemode"] = int(left_lane) + (int(right_lane) << 1)
values["CF_Lkas_LdwsOpt_USM"] = 2

@ -45,7 +45,7 @@ class CarInterface(CarInterfaceBase):
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.mass = 3982. * CV.LB_TO_KG + STD_CARGO_KG
ret.wheelbase = 2.766
@ -347,8 +347,9 @@ class CarInterface(CarInterfaceBase):
return self.CS.out
def apply(self, c):
can_sends = self.CC.update(c.enabled, self.CS, self.frame, c.actuators,
c.cruiseControl.cancel, c.hudControl.visualAlert, c.hudControl.setSpeed, c.hudControl.leftLaneVisible,
c.hudControl.rightLaneVisible, c.hudControl.leftLaneDepart, c.hudControl.rightLaneDepart)
hud_control = c.hudControl
ret = self.CC.update(c.enabled, self.CS, self.frame, c.actuators,
c.cruiseControl.cancel, hud_control.visualAlert, hud_control.setSpeed, hud_control.leftLaneVisible,
hud_control.rightLaneVisible, hud_control.leftLaneDepart, hud_control.rightLaneDepart)
self.frame += 1
return can_sends
return ret

@ -10,13 +10,14 @@ class CarControllerParams:
ACCEL_MAX = 2.0 # m/s
def __init__(self, CP):
if CP.carFingerprint in [CAR.SONATA, CAR.PALISADE, CAR.SANTA_FE, CAR.VELOSTER, CAR.GENESIS_G70, CAR.GENESIS_G70_2020,
CAR.IONIQ_EV_2020, CAR.KIA_CEED, CAR.KIA_SELTOS, CAR.ELANTRA_2021,
CAR.ELANTRA_HEV_2021, CAR.SONATA_HYBRID, CAR.KONA_HEV, CAR.SANTA_FE_2022,
CAR.KIA_K5_2021, CAR.KONA_EV, CAR.KONA, CAR.IONIQ_HEV_2022, CAR.SANTA_FE_HEV_2022]:
self.STEER_MAX = 384
else:
# To determine the limit for your car, find the maximum value that the stock LKAS will request.
# If the max stock LKAS request is <384, add your car to this list.
if CP.carFingerprint in [CAR.GENESIS_G80, CAR.GENESIS_G90, CAR.ELANTRA, CAR.HYUNDAI_GENESIS, CAR.ELANTRA_GT_I30, CAR.IONIQ,
CAR.IONIQ_EV_LTD, CAR.IONIQ_PHEV, CAR.SANTA_FE_PHEV_2022, CAR.SONATA_LF, CAR.KIA_FORTE, CAR.KIA_NIRO_EV,
CAR.KIA_NIRO_HEV, CAR.KIA_NIRO_HEV_2021, CAR.KIA_OPTIMA_H, CAR.KIA_OPTIMA, CAR.KIA_SORENTO, CAR.KIA_STINGER]:
self.STEER_MAX = 255
else:
self.STEER_MAX = 384
self.STEER_DELTA_UP = 3
self.STEER_DELTA_DOWN = 7
self.STEER_DRIVER_ALLOWANCE = 50
@ -41,6 +42,7 @@ class CAR:
SANTA_FE = "HYUNDAI SANTA FE 2019"
SANTA_FE_2022 = "HYUNDAI SANTA FE 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_LF = "HYUNDAI SONATA 2019"
PALISADE = "HYUNDAI PALISADE 2020"
@ -498,6 +500,23 @@ FW_VERSIONS = {
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: {
(Ecu.fwdRadar, 0x7d0, None): [
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.02 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\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 ',
],
(Ecu.esp, 0x7D1, None): [
b'\xf1\x00OS IEB \r 212 \x11\x13 58520-K4000',
b'\xf1\x8799110Q4600\xf1\x00DEev SCC FNCUP 1.00 1.00 99110-Q4600 ',
b'\xf1\x8799110Q4600\xf1\x00DEev SCC FHCUP 1.00 1.00 99110-Q4600 ',
],
(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 56310Q4100\x00 4DEEC105',
b'\xf1\x00OS MDPS C 1.00 1.04 56310K4050\x00 4OEDC104',
],
(Ecu.fwdCamera, 0x7C4, None): [
b'\xf1\000DEE MFC AT EUR LHD 1.00 1.00 99211-Q4100 200706',
b'\xf1\x00DEE MFC AT EUR LHD 1.00 1.00 99211-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\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: {
@ -981,25 +997,25 @@ FW_VERSIONS = {
}
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],
}
FEATURES = {
# which message has the gear
"use_cluster_gears": set([CAR.ELANTRA, CAR.ELANTRA_GT_I30, CAR.KONA]),
"use_tcu_gears": set([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_cluster_gears": {CAR.ELANTRA, CAR.ELANTRA_GT_I30, CAR.KONA},
"use_tcu_gears": {CAR.KIA_OPTIMA, CAR.SONATA_LF, CAR.VELOSTER},
"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
"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
EV_CAR = set([CAR.IONIQ_EV_2020, CAR.IONIQ_EV_LTD, CAR.KONA_EV, CAR.KIA_NIRO_EV])
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 = {CAR.IONIQ_EV_2020, CAR.IONIQ_EV_LTD, CAR.KONA_EV, CAR.KIA_NIRO_EV}
# these cars require a special panda safety mode due to missing counters and checksums in the messages
LEGACY_SAFETY_MODE_CAR = 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 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_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_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'),

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

@ -58,4 +58,8 @@ class CarController():
# send steering command
can_sends.append(mazdacan.create_steering_control(self.packer, CS.CP.carFingerprint,
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
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
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'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'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'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-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'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',
@ -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-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-F\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.esp, 0x760, None): [
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-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-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-R\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.transmission, 0x7e1, None): [
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'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-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-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-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'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'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',
],
(Ecu.engine, 0x7e0, None): [
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'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'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): [
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-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',
],
(Ecu.transmission, 0x7e1, None): [
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'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-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
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
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):
# 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
))
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
def apply(self, c):
can_sends = self.CC.update(c.enabled, self.CS, self.frame, c.actuators,
c.cruiseControl.cancel, c.hudControl.visualAlert,
c.hudControl.leftLaneVisible, c.hudControl.rightLaneVisible,
c.hudControl.leftLaneDepart, c.hudControl.rightLaneDepart)
hud_control = c.hudControl
ret = self.CC.update(c.enabled, self.CS, self.frame, c.actuators,
c.cruiseControl.cancel, hud_control.visualAlert,
hud_control.leftLaneVisible, hud_control.rightLaneVisible,
hud_control.leftLaneDepart, hud_control.rightLaneDepart)
self.frame += 1
return can_sends
return ret

@ -8,11 +8,11 @@ class CarController():
def __init__(self, dbc_name, CP, VM):
self.apply_steer_last = 0
self.es_distance_cnt = -1
self.es_accel_cnt = -1
self.es_lkas_cnt = -1
self.cruise_button_prev = 0
self.steer_rate_limited = False
self.p = CarControllerParams(CP)
self.packer = CANPacker(DBC[CP.carFingerprint]['pt'])
def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, visual_alert, left_line, right_line, left_lane_depart, right_lane_depart):
@ -20,23 +20,23 @@ class CarController():
can_sends = []
# *** 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
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
if not enabled:
apply_steer = 0
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:
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
@ -44,7 +44,7 @@ class CarController():
# *** alerts and pcm cancel ***
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
# disengage ACC when OP is disengaged
if pcm_cancel_cmd:
@ -60,8 +60,8 @@ class CarController():
cruise_button = 0
self.cruise_button_prev = cruise_button
can_sends.append(subarucan.create_es_throttle_control(self.packer, cruise_button, CS.es_accel_msg))
self.es_accel_cnt = CS.es_accel_msg["Counter"]
can_sends.append(subarucan.create_preglobal_es_distance(self.packer, cruise_button, CS.es_distance_msg))
self.es_distance_cnt = CS.es_distance_msg["Counter"]
else:
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))
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
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.es_accel_msg = copy.copy(cp_cam.vl["ES_CruiseThrottle"])
else:
ret.steerWarning = cp.vl["Steering_Torque"]["Steer_Warning"] == 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_distance_msg = copy.copy(cp_cam.vl["ES_Distance"])
return ret
@ -168,28 +167,28 @@ class CarState(CarStateBase):
("Cruise_Set_Speed", "ES_DashStatus", 0),
("Not_Ready_Startup", "ES_DashStatus", 0),
("Throttle_Cruise", "ES_CruiseThrottle", 0),
("Signal1", "ES_CruiseThrottle", 0),
("Cruise_Activated", "ES_CruiseThrottle", 0),
("Signal2", "ES_CruiseThrottle", 0),
("Brake_On", "ES_CruiseThrottle", 0),
("Distance_Swap", "ES_CruiseThrottle", 0),
("Standstill", "ES_CruiseThrottle", 0),
("Signal3", "ES_CruiseThrottle", 0),
("Close_Distance", "ES_CruiseThrottle", 0),
("Signal4", "ES_CruiseThrottle", 0),
("Standstill_2", "ES_CruiseThrottle", 0),
("Cruise_Fault", "ES_CruiseThrottle", 0),
("Signal5", "ES_CruiseThrottle", 0),
("Counter", "ES_CruiseThrottle", 0),
("Signal6", "ES_CruiseThrottle", 0),
("Cruise_Button", "ES_CruiseThrottle", 0),
("Signal7", "ES_CruiseThrottle", 0),
("Cruise_Throttle", "ES_Distance", 0),
("Signal1", "ES_Distance", 0),
("Car_Follow", "ES_Distance", 0),
("Signal2", "ES_Distance", 0),
("Brake_On", "ES_Distance", 0),
("Distance_Swap", "ES_Distance", 0),
("Standstill", "ES_Distance", 0),
("Signal3", "ES_Distance", 0),
("Close_Distance", "ES_Distance", 0),
("Signal4", "ES_Distance", 0),
("Standstill_2", "ES_Distance", 0),
("Cruise_Fault", "ES_Distance", 0),
("Signal5", "ES_Distance", 0),
("Counter", "ES_Distance", 0),
("Signal6", "ES_Distance", 0),
("Cruise_Button", "ES_Distance", 0),
("Signal7", "ES_Distance", 0),
]
checks = [
("ES_DashStatus", 20),
("ES_CruiseThrottle", 20),
("ES_Distance", 20),
]
else:
signals = [

@ -45,6 +45,16 @@ class CarInterface(CarInterfaceBase):
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]]
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:
ret.mass = 1568. + STD_CARGO_KG
ret.wheelbase = 2.67
@ -112,8 +122,9 @@ class CarInterface(CarInterfaceBase):
return self.CS.out
def apply(self, c):
can_sends = self.CC.update(c.enabled, self.CS, self.frame, c.actuators,
c.cruiseControl.cancel, c.hudControl.visualAlert,
c.hudControl.leftLaneVisible, c.hudControl.rightLaneVisible, c.hudControl.leftLaneDepart, c.hudControl.rightLaneDepart)
hud_control = c.hudControl
ret = self.CC.update(c.enabled, self.CS, self.frame, c.actuators,
c.cruiseControl.cancel, hud_control.visualAlert,
hud_control.leftLaneVisible, hud_control.rightLaneVisible, hud_control.leftLaneDepart, hud_control.rightLaneDepart)
self.frame += 1
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)
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["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
class CarControllerParams:
STEER_MAX = 2047 # max_steer 4095
STEER_STEP = 2 # how often we update the steer cmd
STEER_DELTA_UP = 50 # torque increase per refresh, 0.8s to max
STEER_DELTA_DOWN = 70 # torque decrease per refresh
STEER_DRIVER_ALLOWANCE = 60 # allowed driver torque before start limiting
STEER_DRIVER_MULTIPLIER = 10 # weight driver torque heavily
STEER_DRIVER_FACTOR = 1 # from dbc
def __init__(self, CP):
if CP.carFingerprint == CAR.IMPREZA_2020:
self.STEER_MAX = 1439
else:
self.STEER_MAX = 2047
self.STEER_STEP = 2 # how often we update the steer cmd
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:
ASCENT = "SUBARU ASCENT LIMITED 2019"
IMPREZA = "SUBARU IMPREZA LIMITED 2019"
IMPREZA_2020 = "SUBARU IMPREZA SPORT 2020"
FORESTER = "SUBARU FORESTER 2019"
FORESTER_PREGLOBAL = "SUBARU FORESTER 2017 - 2018"
LEGACY_PREGLOBAL = "SUBARU LEGACY 2015 - 2018"
@ -30,6 +35,14 @@ FINGERPRINTS = {
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
}],
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: [{
# 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
@ -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 = {
CAR.ASCENT: 80,
CAR.IMPREZA: 80,
CAR.IMPREZA_2020: 80,
CAR.FORESTER: 80,
CAR.FORESTER_PREGLOBAL: 75,
CAR.LEGACY_PREGLOBAL: 75,
@ -81,6 +142,7 @@ STEER_THRESHOLD = {
DBC = {
CAR.ASCENT: 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_PREGLOBAL: dbc_dict('subaru_forester_2017_generated', None),
CAR.LEGACY_PREGLOBAL: dbc_dict('subaru_outback_2015_generated', None),

@ -62,4 +62,7 @@ class CarController():
# 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
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
return can_sends
return ret

@ -59,7 +59,7 @@ class TestCarInterfaces(unittest.TestCase):
car_interface.apply(CC)
# 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)
assert radar_interface

@ -19,12 +19,12 @@ class CarController():
self.steer_rate_limited = False
self.packer = CANPacker(dbc_name)
self.gas = 0
self.accel = 0
def update(self, enabled, active, CS, frame, actuators, pcm_cancel_cmd, hud_alert,
left_line, right_line, lead, left_lane_depart, right_lane_depart):
# *** compute control surfaces ***
# gas and brake
if CS.CP.enableGasInterceptor and enabled:
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
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
if pcm_cancel_cmd and CS.CP.carFingerprint in [CAR.LEXUS_IS, CAR.LEXUS_RC]:
can_sends.append(create_acc_cancel_command(self.packer))
elif CS.CP.openpilotLongitudinalControl:
can_sends.append(create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.standstill_req, lead, CS.acc_type))
self.accel = pcm_accel_cmd
else:
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.
# This prevents unexpected pedal range rescaling
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:
# - there is something to display
@ -120,15 +122,19 @@ class CarController():
send_ui = True
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:
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:
if frame % fr_step == 0 and CS.CP.enableDsu and CS.CP.carFingerprint in cars:
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
set_lat_tune(ret.lateralTuning, LatTunes.PID_A)
elif candidate == CAR.LEXUS_RX:
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:
elif candidate in [CAR.LEXUS_RX, CAR.LEXUS_RXH, CAR.LEXUS_RX_TSS2, CAR.LEXUS_RXH_TSS2]:
stop_and_go = True
ret.wheelbase = 2.79
ret.steerRatio = 16. # 14.8 is spec end-to-end
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
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]:
stop_and_go = True
ret.wheelbase = 2.63906
@ -312,12 +287,12 @@ class CarInterface(CarInterfaceBase):
# pass in a car.CarControl
# to be called @ 100hz
def apply(self, c):
can_sends = self.CC.update(c.enabled, c.active, self.CS, self.frame,
c.actuators, c.cruiseControl.cancel,
c.hudControl.visualAlert, c.hudControl.leftLaneVisible,
c.hudControl.rightLaneVisible, c.hudControl.leadVisible,
c.hudControl.leftLaneDepart, c.hudControl.rightLaneDepart)
hud_control = c.hudControl
ret = self.CC.update(c.enabled, c.active, self.CS, self.frame,
c.actuators, c.cruiseControl.cancel,
hud_control.visualAlert, hud_control.leftLaneVisible,
hud_control.rightLaneVisible, hud_control.leadVisible,
hud_control.leftLaneDepart, hud_control.rightLaneDepart)
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)
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 = {
"RIGHT_LINE": 3 if right_lane_depart else 1 if right_line else 2,
"LEFT_LINE": 3 if left_lane_depart else 1 if left_line else 2,
"BARRIERS" : 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_X2C": 0x2c,
"SET_ME_X38": 0x38,

@ -80,10 +80,6 @@ def set_lat_tune(tune, name):
tune.pid.kpV = [0.2]
tune.pid.kiV = [0.05]
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:
tune.pid.kpV = [0.6]
tune.pid.kiV = [0.1]
@ -92,10 +88,6 @@ def set_lat_tune(tune, name):
tune.pid.kpV = [0.6]
tune.pid.kiV = [0.1]
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:
tune.pid.kpV = [0.723]
tune.pid.kiV = [0.0428]

@ -536,6 +536,7 @@ FW_VERSIONS = {
b'\x018966312P9200\x00\x00\x00\x00',
b'\x018966312P9300\x00\x00\x00\x00',
b'\x018966312Q2300\x00\x00\x00\x00',
b'\x018966312Q8000\x00\x00\x00\x00',
b'\x018966312R0000\x00\x00\x00\x00',
b'\x018966312R0100\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'\x01F152612B61\x00\x00\x00\x00\x00\x00',
b'\x01F152612B71\x00\x00\x00\x00\x00\x00',
b'\x01F152612B81\x00\x00\x00\x00\x00\x00',
b'\x01F152612B90\x00\x00\x00\x00\x00\x00',
b'\x01F152612C00\x00\x00\x00\x00\x00\x00',
b'F152602191\x00\x00\x00\x00\x00\x00',
@ -768,6 +770,7 @@ FW_VERSIONS = {
b'\x01896630EB2100\x00\x00\x00\x00',
b'\x01896630EB2200\x00\x00\x00\x00',
b'\x01896630EC4000\x00\x00\x00\x00',
b'\x01896630ED9000\x00\x00\x00\x00',
b'\x01896630EE1000\x00\x00\x00\x00',
],
(Ecu.fwdRadar, 0x750, 0xf): [
@ -797,6 +800,7 @@ FW_VERSIONS = {
b'\x02896630E66000\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'\x02896630E66100\x00\x00\x00\x00897CF4801001\x00\x00\x00\x00',
],
(Ecu.fwdRadar, 0x750, 0xf): [
b'\x018821F3301400\x00\x00\x00\x00',
@ -1552,6 +1556,7 @@ FW_VERSIONS = {
},
CAR.PRIUS_TSS2: {
(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'\x028966347C8000\x00\x00\x00\x008966A4703000\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'F152647520\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): [
b'8965B47070\x00\x00\x00\x00\x00\x00',
@ -1636,11 +1642,11 @@ DBC = {
# 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.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_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.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
else:
raise ValueError("unsupported car %s" % candidate)
raise ValueError(f"unsupported car {candidate}")
ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase)
ret.centerToFront = ret.wheelbase * 0.45
@ -216,11 +216,12 @@ class CarInterface(CarInterfaceBase):
return self.CS.out
def apply(self, c):
can_sends = self.CC.update(c.enabled, self.CS, self.frame, self.ext_bus, c.actuators,
c.hudControl.visualAlert,
c.hudControl.leftLaneVisible,
c.hudControl.rightLaneVisible,
c.hudControl.leftLaneDepart,
c.hudControl.rightLaneDepart)
hud_control = c.hudControl
ret = self.CC.update(c.enabled, self.CS, self.frame, self.ext_bus, c.actuators,
hud_control.visualAlert,
hud_control.leftLaneVisible,
hud_control.rightLaneVisible,
hud_control.leftLaneDepart,
hud_control.rightLaneDepart)
self.frame += 1
return can_sends
return ret

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

@ -10,33 +10,19 @@ const int LON_MPC_N = 32;
const float MIN_DRAW_DISTANCE = 10.0;
const float MAX_DRAW_DISTANCE = 100.0;
template<typename T_SRC, typename T_DST, size_t size>
const std::array<T_DST, size> convert_array_to_type(const std::array<T_SRC, size> &src) {
std::array<T_DST, size> dst = {};
for (int i=0; i<size; i++) {
dst[i] = src[i];
template <typename T, size_t size>
constexpr std::array<T, size> build_idxs(float max_val) {
std::array<T, size> result{};
for (int i = 0; i < size; ++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 = {
0. , 0.00976562, 0.0390625 , 0.08789062, 0.15625 ,
0.24414062, 0.3515625 , 0.47851562, 0.625 , 0.79101562,
0.9765625 , 1.18164062, 1.40625 , 1.65039062, 1.9140625 ,
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);
constexpr auto T_IDXS = build_idxs<double, TRAJECTORY_SIZE>(10.0);
constexpr auto T_IDXS_FLOAT = build_idxs<float, TRAJECTORY_SIZE>(10.0);
constexpr auto X_IDXS = build_idxs<double, TRAJECTORY_SIZE>(192.0);
constexpr auto X_IDXS_FLOAT = build_idxs<float, TRAJECTORY_SIZE>(192.0);
const int TICI_CAM_WIDTH = 1928;

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

@ -20,6 +20,8 @@
#include <sched.h>
#endif // __linux__
namespace util {
void set_thread_name(const char* name) {
#ifdef __linux__
// pthread_setname_np is dumb (fails instead of truncates)
@ -56,8 +58,6 @@ int set_core_affinity(std::vector<int> cores) {
#endif
}
namespace util {
std::string read_file(const std::string& fn) {
std::ifstream f(fn, std::ios::binary | std::ios::in);
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_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_core_affinity(std::vector<int> cores);
namespace util {
// ***** Time helpers *****
struct tm get_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.manager.process_config import managed_processes
SOFT_DISABLE_TIME = 3 # seconds
LDW_MIN_SPEED = 31 * CV.MPH_TO_MS
LANE_DEPARTURE_THRESHOLD = 0.1
STEER_ANGLE_SATURATION_TIMEOUT = 1.0 / DT_CTRL
@ -149,7 +150,7 @@ class Controls:
self.v_cruise_kph_last = 0
self.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.saturated_count = 0
self.distance_traveled = 0
@ -158,6 +159,7 @@ class Controls:
self.current_alert_types = [ET.PERMANENT]
self.logged_comm_issue = False
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
self.sm['liveParameters'].valid = True
@ -251,7 +253,7 @@ class Controls:
LaneChangeState.laneChangeFinishing]:
self.events.add(EventName.laneChange)
if self.can_rcv_error or not CS.canValid:
if not CS.canValid:
self.events.add(EventName.canError)
for i, pandaState in enumerate(self.sm['pandaStates']):
@ -271,7 +273,7 @@ class Controls:
self.events.add(EventName.radarFault)
elif not self.sm.valid["pandaStates"]:
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)
if not self.logged_comm_issue:
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
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
if self.cruise_mismatch_counter > int(1. / DT_CTRL):
if self.cruise_mismatch_counter > int(3. / DT_CTRL):
self.events.add(EventName.cruiseMismatch)
# Check for FCW
@ -344,7 +346,7 @@ class Controls:
self.events.add(EventName.localizerMalfunction)
# 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):
self.events.add(EventName.processNotRunning)
@ -376,7 +378,7 @@ class Controls:
# Check for CAN timeout
if not can_strs:
self.can_error_counter += 1
self.can_rcv_error_counter += 1
self.can_rcv_error = True
else:
self.can_rcv_error = False
@ -430,7 +432,7 @@ class Controls:
if self.state == State.enabled:
if self.events.any(ET.SOFT_DISABLE):
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)
# SOFT DISABLING
@ -509,7 +511,7 @@ class Controls:
lat_plan.psis,
lat_plan.curvatures,
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)
else:
lac_log = log.ControlsState.LateralDebugState.new_message()
@ -577,59 +579,66 @@ class Controls:
CC.active = self.active
CC.actuators = actuators
if len(self.sm['liveLocationKalman'].orientationNED.value) > 2:
CC.roll = self.sm['liveLocationKalman'].orientationNED.value[0]
CC.pitch = self.sm['liveLocationKalman'].orientationNED.value[1]
orientation_value = self.sm['liveLocationKalman'].orientationNED.value
if len(orientation_value) > 2:
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)
if self.joystick_mode and self.sm.rcv_frame['testJoystick'] > 0 and self.sm['testJoystick'].buttons[0]:
CC.cruiseControl.cancel = True
CC.hudControl.setSpeed = float(self.v_cruise_kph * CV.KPH_TO_MS)
CC.hudControl.speedVisible = self.enabled
CC.hudControl.lanesVisible = self.enabled
CC.hudControl.leadVisible = self.sm['longitudinalPlan'].hasLead
hudControl = CC.hudControl
hudControl.setSpeed = float(self.v_cruise_kph * CV.KPH_TO_MS)
hudControl.speedVisible = self.enabled
hudControl.lanesVisible = self.enabled
hudControl.leadVisible = self.sm['longitudinalPlan'].hasLead
CC.hudControl.rightLaneVisible = True
CC.hudControl.leftLaneVisible = True
hudControl.rightLaneVisible = True
hudControl.leftLaneVisible = True
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 \
and not self.active and self.sm['liveCalibration'].calStatus == Calibration.CALIBRATED
meta = self.sm['modelV2'].meta
if len(meta.desirePrediction) and ldw_allowed:
model_v2 = self.sm['modelV2']
desire_prediction = model_v2.meta.desirePrediction
if len(desire_prediction) and ldw_allowed:
right_lane_visible = self.sm['lateralPlan'].rProb > 0.5
left_lane_visible = self.sm['lateralPlan'].lProb > 0.5
l_lane_change_prob = meta.desirePrediction[Desire.laneChangeLeft - 1]
r_lane_change_prob = meta.desirePrediction[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))
l_lane_change_prob = desire_prediction[Desire.laneChangeLeft - 1]
r_lane_change_prob = desire_prediction[Desire.laneChangeRight - 1]
CC.hudControl.leftLaneDepart = bool(l_lane_change_prob > LANE_DEPARTURE_THRESHOLD and l_lane_close)
CC.hudControl.rightLaneDepart = bool(r_lane_change_prob > LANE_DEPARTURE_THRESHOLD and r_lane_close)
lane_lines = model_v2.laneLines
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)
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.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:
# 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))
CC.actuatorsOutput = self.last_actuators
force_decel = (self.sm['driverMonitoringState'].awarenessStatus < 0.) or \
(self.state == State.softDisabling)
# Curvature & Steering angle
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
dat = messaging.new_message('controlsState')
@ -659,18 +668,20 @@ class Controls:
controlsState.cumLagMs = -self.rk.remaining * 1000.
controlsState.startMonoTime = int(start_time * 1e9)
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:
controlsState.lateralControlState.debugState = lac_log
elif self.CP.steerControlType == car.CarParams.SteerControlType.angle:
controlsState.lateralControlState.angleState = lac_log
elif self.CP.lateralTuning.which() == 'pid':
elif lat_tuning == 'pid':
controlsState.lateralControlState.pidState = lac_log
elif self.CP.lateralTuning.which() == 'lqr':
elif lat_tuning == 'lqr':
controlsState.lateralControlState.lqrState = lac_log
elif self.CP.lateralTuning.which() == 'indi':
elif lat_tuning == 'indi':
controlsState.lateralControlState.indiState = lac_log
self.pm.send('controlsState', dat)
# carState

@ -1,5 +1,5 @@
from enum import IntEnum
from typing import Dict, Union, Callable, Any
from typing import Dict, Union, Callable, List, Optional
from cereal import log, car
import cereal.messaging as messaging
@ -42,33 +42,33 @@ EVENT_NAME = {v: k for k, v in EventName.schema.enumerants.items()}
class Events:
def __init__(self):
self.events = []
self.static_events = []
self.events: List[int] = []
self.static_events: List[int] = []
self.events_prev = dict.fromkeys(EVENTS.keys(), 0)
@property
def names(self):
def names(self) -> List[int]:
return self.events
def __len__(self):
def __len__(self) -> int:
return len(self.events)
def add(self, event_name, static=False):
def add(self, event_name: int, static: bool=False) -> None:
if static:
self.static_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 = self.static_events.copy()
def any(self, event_type):
def any(self, event_type: str) -> bool:
for e in self.events:
if event_type in EVENTS.get(e, {}).keys():
return True
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:
callback_args = []
@ -129,7 +129,7 @@ class Alert:
self.creation_delay = creation_delay
self.alert_type = ""
self.event_type = None
self.event_type: Optional[str] = None
def __str__(self) -> str:
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):
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,
AlertSize.mid, Priority.LOW, visual_alert,
AudibleAlert.refuse, 3.)
class SoftDisableAlert(Alert):
def __init__(self, alert_text_2):
def __init__(self, alert_text_2: str):
super().__init__("TAKE CONTROL IMMEDIATELY", alert_text_2,
AlertStatus.userPrompt, AlertSize.full,
Priority.MID, VisualAlert.steerRequired,
@ -155,13 +155,13 @@ class SoftDisableAlert(Alert):
# less harsh version of SoftDisable, where the condition is user-triggered
class UserSoftDisableAlert(SoftDisableAlert):
def __init__(self, alert_text_2):
def __init__(self, alert_text_2: str):
super().__init__(alert_text_2),
self.alert_text_1 = "openpilot will disengage"
class ImmediateDisableAlert(Alert):
def __init__(self, alert_text_2):
def __init__(self, alert_text_2: str):
super().__init__("TAKE CONTROL IMMEDIATELY", alert_text_2,
AlertStatus.critical, AlertSize.full,
Priority.HIGHEST, VisualAlert.steerRequired,
@ -198,11 +198,31 @@ def get_display_speed(speed_ms: float, metric: bool) -> str:
# ********** 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)}")
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(
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)
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(
"Calibration in Progress: %d%%" % sm['liveCalibration'].calPerc,
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)
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]
return Alert(
"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.)
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"
if CP.carName == "honda":
text = "Main Switch Off"
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
gb, steer = list(axes)[:2] if len(axes) else (0., 0.)
vals = f"Gas: {round(gb * 100.)}%, Steer: {round(steer * 100.)}%"
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 **********
EventName.stockFcw: {},
EventName.lkasDisabled: {},
# ********** events only containing alerts displayed in all states **********
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
EventName.vehicleModelInvalid: {
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: {
@ -544,7 +567,7 @@ EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, boo
},
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"),
},
@ -581,12 +604,12 @@ EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, boo
EventName.overheat: {
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"),
},
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"),
},
@ -597,33 +620,33 @@ EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, boo
# See https://comma.ai/setup for more information
EventName.calibrationInvalid: {
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"),
},
EventName.calibrationIncomplete: {
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"),
},
EventName.doorOpen: {
ET.SOFT_DISABLE: UserSoftDisableAlert("Door Open"),
ET.SOFT_DISABLE: user_soft_disable_alert("Door Open"),
ET.NO_ENTRY: NoEntryAlert("Door Open"),
},
EventName.seatbeltNotLatched: {
ET.SOFT_DISABLE: UserSoftDisableAlert("Seatbelt Unlatched"),
ET.SOFT_DISABLE: user_soft_disable_alert("Seatbelt Unlatched"),
ET.NO_ENTRY: NoEntryAlert("Seatbelt Unlatched"),
},
EventName.espDisabled: {
ET.SOFT_DISABLE: SoftDisableAlert("ESP Off"),
ET.SOFT_DISABLE: soft_disable_alert("ESP Off"),
ET.NO_ENTRY: NoEntryAlert("ESP Off"),
},
EventName.lowBattery: {
ET.SOFT_DISABLE: SoftDisableAlert("Low Battery"),
ET.SOFT_DISABLE: soft_disable_alert("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
# ten times the regular interval, or the average interval is more than 10% too high.
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"),
},
@ -642,7 +665,7 @@ EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, boo
},
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"),
},
@ -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
# thrown when over 20% of frames are dropped.
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"),
},
@ -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
# as a heuristic to warn the driver.
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"),
},
# 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.
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"),
},
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.NO_ENTRY: NoEntryAlert("Low Memory: Reboot Your Device"),
},
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.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
# causing the connection to the panda to be lost
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.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
def parse_model(self, md):
if len(md.laneLines) == 4 and len(md.laneLines[0].t) == TRAJECTORY_SIZE:
self.ll_t = (np.array(md.laneLines[1].t) + np.array(md.laneLines[2].t))/2
lane_lines = md.laneLines
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
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
self.lll_y = np.array(md.laneLines[1].y) - self.camera_offset
self.rll_y = np.array(md.laneLines[2].y) - self.camera_offset
self.lll_y = np.array(lane_lines[1].y) - self.camera_offset
self.rll_y = np.array(lane_lines[2].y) - self.camera_offset
self.lll_prob = md.laneLineProbs[1]
self.rll_prob = md.laneLineProbs[2]
self.lll_std = md.laneLineStds[1]
self.rll_std = md.laneLineStds[2]
if len(md.meta.desireState):
self.l_lane_change_prob = md.meta.desireState[log.LateralPlan.Desire.laneChangeLeft]
self.r_lane_change_prob = md.meta.desireState[log.LateralPlan.Desire.laneChangeRight]
desire_state = md.meta.desireState
if len(desire_state):
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):
# Reduce reliance on lanelines that are too far apart or

@ -10,7 +10,7 @@ class LatControlAngle():
def reset(self):
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()
if CS.vEgo < 0.3 or not active:
@ -18,11 +18,11 @@ class LatControlAngle():
angle_steers_des = float(CS.steeringAngleDeg)
else:
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_log.saturated = False
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

@ -82,7 +82,7 @@ class LatControlINDI():
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
# Update Kalman filter
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.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)
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)
if CS.vEgo < 0.3 or not active:

@ -44,7 +44,7 @@ class LatControlLQR():
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()
steers_max = get_steer_max(CP, CS.vEgo)
@ -53,7 +53,7 @@ class LatControlLQR():
# Subtract offset. Zero angle should correspond to zero torque
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
desired_angle += instant_offset # Only add offset that originates from vehicle model errors

@ -16,15 +16,15 @@ class LatControlPID():
def reset(self):
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.steeringAngleDeg = float(CS.steeringAngleDeg)
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
pid_log.steeringAngleDesiredDeg = angle_steers_des
pid_log.steeringAngleDesiredDeg = angle_steers_des
pid_log.angleError = angle_steers_des - CS.steeringAngleDeg
if CS.vEgo < 0.3 or not active:
output_steer = 0.0

@ -48,12 +48,13 @@ lenv.Clean(generated_files, Dir(gen))
lenv.Command(generated_files,
["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["CXXFLAGS"].append("-DACADOS_WITH_QPOASES")
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",
build_files,
LIBS=['m', 'acados', 'hpipm', 'blasfeo', 'qpOASES_e'])

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

@ -7,21 +7,17 @@ from selfdrive.modeld.constants import T_IDXS
LongCtrlState = car.CarControl.Actuators.LongControlState
STOPPING_TARGET_SPEED_OFFSET = 0.01
# As per ISO 15622:2018 for all speeds
ACCEL_MIN_ISO = -3.5 # 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,
output_accel, brake_pressed, cruise_standstill, min_speed_can):
def long_control_state_trans(CP, active, long_control_state, v_ego, v_target_future,
output_accel, brake_pressed, cruise_standstill):
"""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 \
(v_ego < CP.vEgoStopping and
((v_pid < stopping_target_speed and v_target_future < stopping_target_speed) or
brake_pressed))
(v_target_future < CP.vEgoStopping or brake_pressed))
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"""
# Interp control trajectory
# TODO estimate car specific lag, use .15s for now
if len(long_plan.speeds) == CONTROL_N:
v_target_lower = interp(CP.longitudinalActuatorDelayLowerBound, T_IDXS[:CONTROL_N], long_plan.speeds)
a_target_lower = 2 * (v_target_lower - long_plan.speeds[0])/CP.longitudinalActuatorDelayLowerBound - long_plan.accels[0]
speeds = long_plan.speeds
if len(speeds) == CONTROL_N:
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)
a_target_upper = 2 * (v_target_upper - long_plan.speeds[0])/CP.longitudinalActuatorDelayUpperBound - long_plan.accels[0]
v_target_upper = interp(CP.longitudinalActuatorDelayUpperBound, T_IDXS[:CONTROL_N], speeds)
a_target_upper = 2 * (v_target_upper - speeds[0])/CP.longitudinalActuatorDelayUpperBound - long_plan.accels[0]
a_target = min(a_target_lower, a_target_upper)
v_target_future = long_plan.speeds[-1]
v_target_future = speeds[-1]
else:
v_target_future = 0.0
a_target = 0.0
@ -91,8 +88,8 @@ class LongControl():
# Update state machine
output_accel = self.last_output_accel
self.long_control_state = long_control_state_trans(CP, active, self.long_control_state, CS.vEgo,
v_target_future, self.v_pid, output_accel,
CS.brakePressed, CS.cruiseState.standstill, CP.minSpeedCan)
v_target_future, output_accel,
CS.brakePressed, CS.cruiseState.standstill)
if self.long_control_state == LongCtrlState.off or CS.gasPressed:
self.reset(CS.vEgo)
@ -100,7 +97,7 @@ class LongControl():
# tracking objects and driving
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
# 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:
output_accel -= CP.stoppingDecelRate * DT_CTRL
output_accel = clip(output_accel, accel_limits[0], accel_limits[1])
self.reset(CS.vEgo)
# 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