Merge branch 'master' into docs-c

.github/workflows/selfdrive_tests.yaml

@@ -351,11 +351,12 @@ jobs:
         name: longitudinal
         path: selfdrive/test/longitudinal_maneuvers/out/longitudinal/
 
-  test_car_models:
+  test_cars:
-    name: car models
+    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

.github/workflows/tools_tests.yaml

@@ -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"

.gitignore

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

.pre-commit-config.yaml

@@ -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++

Dockerfile.openpilot_base

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

Pipfile

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

RELEASES.md

@@ -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

SConstruct

@@ -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"]

cereal

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

common/ffi_wrapper.py

@@ -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:

common/file_helpers.py

@@ -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"
 
 

common/profiler.py

@@ -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}")

common/realtime.py

@@ -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

common/spinner.py

@@ -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):

common/tests/test_file_helpers.py

@@ -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")
 

common/timeout.py

@@ -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
 

docs/CARS.md

@@ -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         |

docs/CONTRIBUTING.md

@@ -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:
+### Automated Testing
-```
+
-git clone https://github.com/commaai/openpilot.git --recursive
+All PRs and commits are automatically checked by GitHub Actions. Check out `.github/workflows/` for what GitHub Actions runs. Any new tests should be added to GitHub Actions.
-```
+
-Or alternatively, when on the master branch:
+### Code Style and Linting
-```
+
-git submodule update --init
+Code is automatically checked for style by GitHub Actions as part of the automated tests. You can also run these tests yourself by running `pre-commit run --all`.
-```
-The reasons for having submodules on a dedicated repository and our new development philosophy can be found in our [post about externalization](https://blog.comma.ai/a-2020-theme-externalization/).
-Modules that are in seperate repositories include:
-* cereal
-* laika
-* opendbc
-* panda
-* rednose

docs/_static/robots.txt

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

docs/conf.py

@@ -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,27 +86,25 @@ 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  = {
+breathe_projects_source = {}
-        # "loggerd" : ("../../../selfdrive/loggerd", ["logger.h"])
-        }
 
 # 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):
+for root, dirs, files in os.walk(BASEDIR):
   found = False
   breath_src = {}
   breathe_srcs_list = []
 
   for file in files:
-                ccFile = os.path.join(root, file)[:-2] +".cc"
+    ccFile = os.path.join(root, file)[:-2] + ".cc"
 
     if file.endswith(".h") and exists(ccFile):
       f = os.path.join(root, file)
@@ -83,11 +113,8 @@ for root, dirs, files in os.walk(OPENPILOT_ROOT):
       print(f"\tFOUND: {f} in {parent_dir} ({parent_dir_abs})")
 
       breathe_srcs_list.append(file)
-                        # breathe_srcs_list.append(ccFile)
       found = True
 
-                # print(f"\tbreathe_srcs_list: {breathe_srcs_list}")
-
     if found:
       breath_src[parent_dir] = (parent_dir_abs, breathe_srcs_list)
       breathe_projects_source.update(breath_src)
@@ -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']

opendbc

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

panda

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

selfdrive/athena/athenad.py

@@ -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)

selfdrive/athena/manage_athenad.py

@@ -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)

selfdrive/athena/tests/helpers.py

@@ -50,18 +50,28 @@ class MockApi():
 
 
 class MockParams():
-  def __init__(self):
+  default_params = {
-    self.params = {
     "DongleId": b"0000000000000000",
-      "GithubSshKeys": b"ssh-rsa AAAAB3NzaC1yc2EAAAADAQABAAABAQC307aE+nuHzTAgaJhzSf5v7ZZQW9gaperjhCmyPyl4PzY7T1mDGenTlVTN7yoVFZ9UfO9oMQqo0n1OwDIiqbIFxqnhrHU0cYfj88rI85m5BEKlNu5RdaVTj1tcbaPpQc5kZEolaI1nDDjzV0lwS7jo5VYDHseiJHlik3HH1SgtdtsuamGR2T80q1SyW+5rHoMOJG73IH2553NnWuikKiuikGHUYBd00K1ilVAK2xSiMWJp55tQfZ0ecr9QjEsJ+J/efL4HqGNXhffxvypCXvbUYAFSddOwXUPo5BTKevpxMtH+2YrkpSjocWA04VnTYFiPG6U4ItKmbLOTFZtPzoez private"  # noqa: E501
+    "GithubSshKeys": b"ssh-rsa AAAAB3NzaC1yc2EAAAADAQABAAABAQC307aE+nuHzTAgaJhzSf5v7ZZQW9gaperjhCmyPyl4PzY7T1mDGenTlVTN7yoVFZ9UfO9oMQqo0n1OwDIiqbIFxqnhrHU0cYfj88rI85m5BEKlNu5RdaVTj1tcbaPpQc5kZEolaI1nDDjzV0lwS7jo5VYDHseiJHlik3HH1SgtdtsuamGR2T80q1SyW+5rHoMOJG73IH2553NnWuikKiuikGHUYBd00K1ilVAK2xSiMWJp55tQfZ0ecr9QjEsJ+J/efL4HqGNXhffxvypCXvbUYAFSddOwXUPo5BTKevpxMtH+2YrkpSjocWA04VnTYFiPG6U4ItKmbLOTFZtPzoez private",  # noqa: E501
+    "AthenadUploadQueue": '[]'
   }
+  params = default_params.copy()
+
+  @staticmethod
+  def restore_defaults():
+    MockParams.params = MockParams.default_params.copy()
 
   def get(self, k, encoding=None):
-    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):

selfdrive/athena/tests/test_athenad.py

@@ -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()

selfdrive/boardd/boardd.cc

@@ -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();
+  std::unique_ptr<Context> context(Context::create());
-  SubSocket * subscriber = SubSocket::create(context, "sendcan");
+  std::unique_ptr<SubSocket> subscriber(SubSocket::create(context.get(), "sendcan"));
   assert(subscriber != NULL);
   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 ((nanos_since_boot() - event.getLogMonoTime() < 1e9) && !fake_send) {
-      if (!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);
   }
 

selfdrive/boardd/panda.cc

@@ -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() {
+void Panda::send_heartbeat(bool engaged) {
-  usb_write(0xf3, 1, 0);
+  usb_write(0xf3, engaged, 0);
 }
 
 void Panda::set_can_speed_kbps(uint16_t bus, uint16_t speed) {

selfdrive/boardd/panda.h

@@ -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);

selfdrive/boardd/tests/test_boardd_loopback.py

@@ -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'))

selfdrive/camerad/cameras/camera_common.cc

@@ -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);
+    s->sm->update(0);
-    driver_cam_auto_exposure(c, *sm);
+    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);
 }

selfdrive/camerad/cameras/camera_common.h

@@ -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);

selfdrive/camerad/cameras/camera_qcom.cc

@@ -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) {
+static void do_autofocus(CameraState *s) {
-  sm->update(0);
-  if(sm->updated("sensorEvents")) {
-    for (auto event : (*sm)["sensorEvents"].getSensorEvents()) {
-      if (event.which() == cereal::SensorEventData::ACCELERATION) {
-        if (auto v = event.getAcceleration().getV(); v.size() >= 3)
-          return -v[2];
-        break;
-      }
-    }
-  }
-  return std::nullopt;
-}
-
-static void do_autofocus(CameraState *s, SubMaster *sm) {
   float lens_true_pos = s->lens_true_pos.load();
   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);
-
+  actuator_move(s, lens_true_pos);
-  /*char debug[4096];
-  char *pdebug = debug;
-  pdebug += sprintf(pdebug, "focus ");
-  for (int i = 0; i < NUM_FOCUS; i++) pdebug += sprintf(pdebug, "%2x(%4d) ", s->confidence[i], s->focus[i]);
-  pdebug += sprintf(pdebug, "  err: %7.2f  offset: %6.2f sag: %6.2f lens_true_pos: %6.2f  cur_lens_pos: %4d->%4d", err * focus_kp, offset, sag, s->lens_true_pos, s->cur_lens_pos, target);
-  LOGD(debug);*/
-
-  actuator_move(s, target);
 }
 
 void camera_autoexposure(CameraState *s, float grey_frac) {
@@ -1045,13 +1018,12 @@ static void ops_thread(MultiCameraState *s) {
   CameraExpInfo road_cam_op;
   CameraExpInfo driver_cam_op;
 
-  set_thread_name("camera_settings");
+  util::set_thread_name("camera_settings");
-  SubMaster sm({"sensorEvents"});
   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,

selfdrive/camerad/cameras/camera_qcom.h

@@ -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;

selfdrive/camerad/cameras/camera_qcom2.cc

@@ -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

selfdrive/camerad/cameras/camera_replay.cc

@@ -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);
 // }
 

selfdrive/camerad/cameras/camera_webcam.cc

@@ -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();

selfdrive/camerad/imgproc/utils.cc

@@ -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)
+LapConv::LapConv(cl_device_id device_id, cl_context ctx, int rgb_width, int rgb_height, int rgb_stride, int filter_size)
-    : width(rgb_width / NUM_SEGMENTS_X), height(rgb_height / NUM_SEGMENTS_Y), 
+    : width(rgb_width / NUM_SEGMENTS_X), height(rgb_height / NUM_SEGMENTS_Y), rgb_stride(rgb_stride),
       roi_buf(width * height * 3), result_buf(width * height) {
 
   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));

selfdrive/camerad/imgproc/utils.h

@@ -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;
 };

selfdrive/camerad/main.cc

@@ -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
   }
 

selfdrive/camerad/test/test_camerad.py

@@ -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

selfdrive/car/car_helpers.py

@@ -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

selfdrive/car/chrysler/carcontroller.py

@@ -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

selfdrive/car/chrysler/interface.py

@@ -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 self.CC.update(c.enabled, self.CS, c.actuators, c.cruiseControl.cancel, c.hudControl.visualAlert)
-
-    return can_sends

selfdrive/car/fingerprints.py

@@ -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

selfdrive/car/ford/carcontroller.py

@@ -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

selfdrive/car/ford/interface.py

@@ -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

selfdrive/car/fw_versions.py

@@ -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")

selfdrive/car/gm/carcontroller.py

@@ -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))
 
+    # Gas/regen and brakes - all at 25Hz
+    if (frame % 4) == 0:
       if not enabled:
         # Stock ECU sends max regen when not enabled.
-      apply_gas = P.MAX_ACC_REGEN
+        self.apply_gas = P.MAX_ACC_REGEN
-      apply_brake = 0
+        self.apply_brake = 0
       else:
-      apply_gas = int(round(interp(actuators.accel, P.GAS_LOOKUP_BP, P.GAS_LOOKUP_V)))
+        self.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)))
+        self.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:
       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_friction_brake_command(self.packer_ch, CanBus.CHASSIS, self.apply_brake, idx, near_stop, at_full_stop))
-      can_sends.append(gmcan.create_gas_regen_command(self.packer_pt, CanBus.POWERTRAIN, apply_gas, idx, enabled, at_full_stop))
+      can_sends.append(gmcan.create_gas_regen_command(self.packer_pt, CanBus.POWERTRAIN, self.apply_gas, idx, enabled, at_full_stop))
 
     # Send dashboard UI commands (ACC status), 25hz
     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

selfdrive/car/gm/carstate.py

@@ -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),

selfdrive/car/gm/interface.py

@@ -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,
+    ret = self.CC.update(enabled, self.CS, self.frame,
                          c.actuators,
-                               hud_v_cruise, c.hudControl.lanesVisible,
+                         hud_v_cruise, hud_control.lanesVisible,
-                               c.hudControl.leadVisible, c.hudControl.visualAlert)
+                         hud_control.leadVisible, hud_control.visualAlert)
 
     self.frame += 1
-    return can_sends
+    return ret

selfdrive/car/gm/radar_interface.py

@@ -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

selfdrive/car/honda/carcontroller.py

@@ -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)
+          self.accel = clip(accel, P.BOSCH_ACCEL_MIN, P.BOSCH_ACCEL_MAX)
-          bosch_gas = interp(accel, P.BOSCH_GAS_LOOKUP_BP, P.BOSCH_GAS_LOOKUP_V)
+          self.gas = interp(accel, P.BOSCH_GAS_LOOKUP_BP, P.BOSCH_GAS_LOOKUP_V)
-          can_sends.extend(hondacan.create_acc_commands(self.packer, enabled, active, accel, bosch_gas, idx, stopping, starting, CS.CP.carFingerprint))
+          can_sends.extend(hondacan.create_acc_commands(self.packer, enabled, active, accel, self.gas, idx, stopping, starting, CS.CP.carFingerprint))
-
         else:
           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
+              self.gas = 0.0
-            can_sends.append(create_gas_interceptor_command(self.packer, apply_gas, idx))
+            can_sends.append(create_gas_interceptor_command(self.packer, self.gas, idx))
-
-    hud = HUDData(int(pcm_accel), int(round(hud_v_cruise)), hud_car,
-                  hud_lanes, fcw_display, acc_alert, steer_required)
 
     # Send dashboard UI commands.
     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

selfdrive/car/honda/interface.py

@@ -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_control = c.hudControl
-      hud_v_cruise = c.hudControl.setSpeed * CV.MS_TO_KPH
+    if hud_control.speedVisible:
+      hud_v_cruise = hud_control.setSpeed * CV.MS_TO_KPH
     else:
       hud_v_cruise = 255
 
-    can_sends = self.CC.update(c.enabled, c.active, self.CS, self.frame,
+    ret = self.CC.update(c.enabled, c.active, self.CS, self.frame,
                          c.actuators,
                          c.cruiseControl.cancel,
                          hud_v_cruise,
-                               c.hudControl.lanesVisible,
+                         hud_control.lanesVisible,
-                               hud_show_car=c.hudControl.leadVisible,
+                         hud_show_car=hud_control.leadVisible,
-                               hud_alert=c.hudControl.visualAlert)
+                         hud_alert=hud_control.visualAlert)
 
     self.frame += 1
-    return can_sends
+    return ret

selfdrive/car/honda/values.py

@@ -1385,9 +1385,9 @@ STEER_THRESHOLD = {
   CAR.CRV_EU: 400,
 }
 
-HONDA_NIDEC_ALT_PCM_ACCEL = set([CAR.ODYSSEY])
+HONDA_NIDEC_ALT_PCM_ACCEL = {CAR.ODYSSEY}
-HONDA_NIDEC_ALT_SCM_MESSAGES = set([CAR.ACURA_ILX, CAR.ACURA_RDX, CAR.CRV, CAR.CRV_EU, CAR.FIT, CAR.FREED, CAR.HRV, CAR.ODYSSEY_CHN,
+HONDA_NIDEC_ALT_SCM_MESSAGES = {CAR.ACURA_ILX, CAR.ACURA_RDX, CAR.CRV, CAR.CRV_EU, CAR.FIT, CAR.FREED, CAR.HRV, CAR.ODYSSEY_CHN,
-                                    CAR.PILOT, CAR.PILOT_2019, CAR.PASSPORT, CAR.RIDGELINE])
+                                    CAR.PILOT, CAR.PILOT_2019, CAR.PASSPORT, CAR.RIDGELINE}
-HONDA_BOSCH = set([CAR.ACCORD, CAR.ACCORDH, CAR.CIVIC_BOSCH, CAR.CIVIC_BOSCH_DIESEL, CAR.CRV_5G,
+HONDA_BOSCH = {CAR.ACCORD, CAR.ACCORDH, CAR.CIVIC_BOSCH, CAR.CIVIC_BOSCH_DIESEL, CAR.CRV_5G,
-                   CAR.CRV_HYBRID, CAR.INSIGHT, CAR.ACURA_RDX_3G, CAR.HONDA_E])
+                   CAR.CRV_HYBRID, CAR.INSIGHT, CAR.ACURA_RDX_3G, CAR.HONDA_E}
-HONDA_BOSCH_ALT_BRAKE_SIGNAL = set([CAR.ACCORD, CAR.CRV_5G, CAR.ACURA_RDX_3G])
+HONDA_BOSCH_ALT_BRAKE_SIGNAL = {CAR.ACCORD, CAR.CRV_5G, CAR.ACURA_RDX_3G}

selfdrive/car/hyundai/carcontroller.py

@@ -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

selfdrive/car/hyundai/hyundaican.py

@@ -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
 

selfdrive/car/hyundai/interface.py

@@ -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,
+    hud_control = c.hudControl
-                               c.cruiseControl.cancel, c.hudControl.visualAlert, c.hudControl.setSpeed, c.hudControl.leftLaneVisible,
+    ret = self.CC.update(c.enabled, self.CS, self.frame, c.actuators,
-                               c.hudControl.rightLaneVisible, c.hudControl.leftLaneDepart, c.hudControl.rightLaneDepart)
+                         c.cruiseControl.cancel, hud_control.visualAlert, hud_control.setSpeed, hud_control.leftLaneVisible,
+                         hud_control.rightLaneVisible, hud_control.leftLaneDepart, hud_control.rightLaneDepart)
     self.frame += 1
-    return can_sends
+    return ret

selfdrive/car/hyundai/values.py

@@ -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,
+    # To determine the limit for your car, find the maximum value that the stock LKAS will request.
-                             CAR.IONIQ_EV_2020, CAR.KIA_CEED, CAR.KIA_SELTOS, CAR.ELANTRA_2021,
+    # If the max stock LKAS request is <384, add your car to this list.
-                             CAR.ELANTRA_HEV_2021, CAR.SONATA_HYBRID, CAR.KONA_HEV, CAR.SANTA_FE_2022,
+    if CP.carFingerprint in [CAR.GENESIS_G80, CAR.GENESIS_G90, CAR.ELANTRA, CAR.HYUNDAI_GENESIS, CAR.ELANTRA_GT_I30, CAR.IONIQ,
-                             CAR.KIA_K5_2021, CAR.KONA_EV, CAR.KONA, CAR.IONIQ_HEV_2022, CAR.SANTA_FE_HEV_2022]:
+                             CAR.IONIQ_EV_LTD, CAR.IONIQ_PHEV, CAR.SANTA_FE_PHEV_2022, CAR.SONATA_LF, CAR.KIA_FORTE, CAR.KIA_NIRO_EV,
-      self.STEER_MAX = 384
+                             CAR.KIA_NIRO_HEV, CAR.KIA_NIRO_HEV_2021, CAR.KIA_OPTIMA_H, CAR.KIA_OPTIMA, CAR.KIA_SORENTO, CAR.KIA_STINGER]:
-    else:
       self.STEER_MAX = 255
+    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         ',
-    ],
+      b'\xf1\x8799110Q4600\xf1\x00DEev SCC FNCUP      1.00 1.00 99110-Q4600         ',
-    (Ecu.esp, 0x7D1, None): [
+      b'\xf1\x8799110Q4600\xf1\x00DEev SCC FHCUP      1.00 1.00 99110-Q4600         ',
-      b'\xf1\x00OS IEB \r 212 \x11\x13 58520-K4000',
     ],
     (Ecu.eps, 0x7D4, None): [
       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_cluster_gears": {CAR.ELANTRA, CAR.ELANTRA_GT_I30, CAR.KONA},
-  "use_tcu_gears": set([CAR.KIA_OPTIMA, CAR.SONATA_LF, CAR.VELOSTER]),
+  "use_tcu_gears": {CAR.KIA_OPTIMA, CAR.SONATA_LF, CAR.VELOSTER},
-  "use_elect_gears": set([CAR.KIA_NIRO_EV, CAR.KIA_NIRO_HEV, CAR.KIA_NIRO_HEV_2021, CAR.KIA_OPTIMA_H, CAR.IONIQ_EV_LTD, CAR.KONA_EV, CAR.IONIQ, CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV, CAR.ELANTRA_HEV_2021,CAR.SONATA_HYBRID, CAR.KONA_HEV, CAR.IONIQ_HEV_2022, CAR.SANTA_FE_HEV_2022]),
+  "use_elect_gears": {CAR.KIA_NIRO_EV, CAR.KIA_NIRO_HEV, CAR.KIA_NIRO_HEV_2021, CAR.KIA_OPTIMA_H, CAR.IONIQ_EV_LTD, CAR.KONA_EV, CAR.IONIQ, CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV, CAR.ELANTRA_HEV_2021,CAR.SONATA_HYBRID, CAR.KONA_HEV, CAR.IONIQ_HEV_2022, CAR.SANTA_FE_HEV_2022, CAR.SANTA_FE_PHEV_2022},
 
   # these cars use the FCA11 message for the AEB and FCW signals, all others use SCC12
-  "use_fca": set([CAR.SONATA, CAR.SONATA_HYBRID, CAR.ELANTRA, CAR.ELANTRA_2021, CAR.ELANTRA_HEV_2021, CAR.ELANTRA_GT_I30, CAR.KIA_STINGER, CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV, CAR.KONA_EV, CAR.KIA_FORTE, CAR.KIA_NIRO_EV, CAR.PALISADE, CAR.GENESIS_G70, CAR.GENESIS_G70_2020, CAR.KONA, CAR.SANTA_FE, CAR.KIA_SELTOS, CAR.KONA_HEV, CAR.SANTA_FE_2022, CAR.KIA_K5_2021, CAR.IONIQ_HEV_2022, CAR.SANTA_FE_HEV_2022]),
+  "use_fca": {CAR.SONATA, CAR.SONATA_HYBRID, CAR.ELANTRA, CAR.ELANTRA_2021, CAR.ELANTRA_HEV_2021, CAR.ELANTRA_GT_I30, CAR.KIA_STINGER, CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV, CAR.KONA_EV, CAR.KIA_FORTE, CAR.KIA_NIRO_EV, CAR.PALISADE, CAR.GENESIS_G70, CAR.GENESIS_G70_2020, CAR.KONA, CAR.SANTA_FE, CAR.KIA_SELTOS, CAR.KONA_HEV, CAR.SANTA_FE_2022, CAR.KIA_K5_2021, CAR.IONIQ_HEV_2022, CAR.SANTA_FE_HEV_2022, CAR.SANTA_FE_PHEV_2022},
 }
 
-HYBRID_CAR = set([CAR.IONIQ_PHEV, CAR.ELANTRA_HEV_2021, CAR.KIA_NIRO_HEV, CAR.KIA_NIRO_HEV_2021, CAR.SONATA_HYBRID, CAR.KONA_HEV, CAR.IONIQ, CAR.IONIQ_HEV_2022, CAR.SANTA_FE_HEV_2022])  # these cars use a different gas signal
+HYBRID_CAR = {CAR.IONIQ_PHEV, CAR.ELANTRA_HEV_2021, CAR.KIA_NIRO_HEV, CAR.KIA_NIRO_HEV_2021, CAR.SONATA_HYBRID, CAR.KONA_HEV, CAR.IONIQ, CAR.IONIQ_HEV_2022, CAR.SANTA_FE_HEV_2022, CAR.SANTA_FE_PHEV_2022}  # these cars use a different gas signal
-EV_CAR = set([CAR.IONIQ_EV_2020, CAR.IONIQ_EV_LTD, CAR.KONA_EV, CAR.KIA_NIRO_EV])
+EV_CAR = {CAR.IONIQ_EV_2020, CAR.IONIQ_EV_LTD, CAR.KONA_EV, CAR.KIA_NIRO_EV}
 
 # these cars require a special panda safety mode due to missing counters and checksums in the messages
-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'),

selfdrive/car/interfaces.py

@@ -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
+  @abstractmethod
-  def update(self, c, can_strings):
+  def update(self, c: car.CarControl, can_strings: List[bytes]) -> car.CarState:
-    raise NotImplementedError
+    pass
 
-  # return sendcan, pass in a car.CarControl
+  @abstractmethod
-  def apply(self, c):
+  def apply(self, c: car.CarControl) -> Tuple[car.CarControl.Actuators, List[bytes]]:
-    raise NotImplementedError
+    pass
 
   def create_common_events(self, cs_out, extra_gears=None, gas_resume_speed=-1, pcm_enable=True):
     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

selfdrive/car/mazda/carcontroller.py

@@ -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

selfdrive/car/mazda/interface.py

@@ -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

selfdrive/car/mazda/values.py

@@ -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}

selfdrive/car/mock/interface.py

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

selfdrive/car/nissan/carcontroller.py

@@ -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

selfdrive/car/nissan/interface.py

@@ -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,
+    hud_control = c.hudControl
-                               c.cruiseControl.cancel, c.hudControl.visualAlert,
+    ret = self.CC.update(c.enabled, self.CS, self.frame, c.actuators,
-                               c.hudControl.leftLaneVisible, c.hudControl.rightLaneVisible,
+                         c.cruiseControl.cancel, hud_control.visualAlert,
-                               c.hudControl.leftLaneDepart, c.hudControl.rightLaneDepart)
+                         hud_control.leftLaneVisible, hud_control.rightLaneVisible,
+                         hud_control.leftLaneDepart, hud_control.rightLaneDepart)
     self.frame += 1
-    return can_sends
+    return ret

selfdrive/car/subaru/carcontroller.py

@@ -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))
+        can_sends.append(subarucan.create_preglobal_es_distance(self.packer, cruise_button, CS.es_distance_msg))
-        self.es_accel_cnt = CS.es_accel_msg["Counter"]
+        self.es_distance_cnt = CS.es_distance_msg["Counter"]
 
     else:
       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

selfdrive/car/subaru/carstate.py

@@ -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),
+        ("Cruise_Throttle", "ES_Distance", 0),
-        ("Signal1", "ES_CruiseThrottle", 0),
+        ("Signal1", "ES_Distance", 0),
-        ("Cruise_Activated", "ES_CruiseThrottle", 0),
+        ("Car_Follow", "ES_Distance", 0),
-        ("Signal2", "ES_CruiseThrottle", 0),
+        ("Signal2", "ES_Distance", 0),
-        ("Brake_On", "ES_CruiseThrottle", 0),
+        ("Brake_On", "ES_Distance", 0),
-        ("Distance_Swap", "ES_CruiseThrottle", 0),
+        ("Distance_Swap", "ES_Distance", 0),
-        ("Standstill", "ES_CruiseThrottle", 0),
+        ("Standstill", "ES_Distance", 0),
-        ("Signal3", "ES_CruiseThrottle", 0),
+        ("Signal3", "ES_Distance", 0),
-        ("Close_Distance", "ES_CruiseThrottle", 0),
+        ("Close_Distance", "ES_Distance", 0),
-        ("Signal4", "ES_CruiseThrottle", 0),
+        ("Signal4", "ES_Distance", 0),
-        ("Standstill_2", "ES_CruiseThrottle", 0),
+        ("Standstill_2", "ES_Distance", 0),
-        ("Cruise_Fault", "ES_CruiseThrottle", 0),
+        ("Cruise_Fault", "ES_Distance", 0),
-        ("Signal5", "ES_CruiseThrottle", 0),
+        ("Signal5", "ES_Distance", 0),
-        ("Counter", "ES_CruiseThrottle", 0),
+        ("Counter", "ES_Distance", 0),
-        ("Signal6", "ES_CruiseThrottle", 0),
+        ("Signal6", "ES_Distance", 0),
-        ("Cruise_Button", "ES_CruiseThrottle", 0),
+        ("Cruise_Button", "ES_Distance", 0),
-        ("Signal7", "ES_CruiseThrottle", 0),
+        ("Signal7", "ES_Distance", 0),
       ]
 
       checks = [
         ("ES_DashStatus", 20),
-        ("ES_CruiseThrottle", 20),
+        ("ES_Distance", 20),
       ]
     else:
       signals = [

selfdrive/car/subaru/interface.py

@@ -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,
+    hud_control = c.hudControl
-                               c.cruiseControl.cancel, c.hudControl.visualAlert,
+    ret = self.CC.update(c.enabled, self.CS, self.frame, c.actuators,
-                               c.hudControl.leftLaneVisible, c.hudControl.rightLaneVisible, c.hudControl.leftLaneDepart, c.hudControl.rightLaneDepart)
+                         c.cruiseControl.cancel, hud_control.visualAlert,
+                         hud_control.leftLaneVisible, hud_control.rightLaneVisible, hud_control.leftLaneDepart, hud_control.rightLaneDepart)
     self.frame += 1
-    return can_sends
+    return ret

selfdrive/car/subaru/subarucan.py

@@ -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)

selfdrive/car/subaru/values.py

@@ -5,17 +5,22 @@ from cereal import car
 Ecu = car.CarParams.Ecu
 
 class CarControllerParams:
-  STEER_MAX = 2047              # max_steer 4095
+  def __init__(self, CP):
-  STEER_STEP = 2                # how often we update the steer cmd
+    if CP.carFingerprint == CAR.IMPREZA_2020:
-  STEER_DELTA_UP = 50           # torque increase per refresh, 0.8s to max
+      self.STEER_MAX = 1439
-  STEER_DELTA_DOWN = 70         # torque decrease per refresh
+    else:
-  STEER_DRIVER_ALLOWANCE = 60   # allowed driver torque before start limiting
+      self.STEER_MAX = 2047
-  STEER_DRIVER_MULTIPLIER = 10  # weight driver torque heavily
+    self.STEER_STEP = 2                # how often we update the steer cmd
-  STEER_DRIVER_FACTOR = 1       # from dbc
+    self.STEER_DELTA_UP = 50           # torque increase per refresh, 0.8s to max
+    self.STEER_DELTA_DOWN = 70         # torque decrease per refresh
+    self.STEER_DRIVER_ALLOWANCE = 60   # allowed driver torque before start limiting
+    self.STEER_DRIVER_MULTIPLIER = 10  # weight driver torque heavily
+    self.STEER_DRIVER_FACTOR = 1       # from dbc
 
 class CAR:
   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),

selfdrive/car/tesla/carcontroller.py

@@ -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

selfdrive/car/tesla/interface.py

@@ -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

selfdrive/car/tests/test_car_interfaces.py

@@ -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
 

selfdrive/car/toyota/carcontroller.py

@@ -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

selfdrive/car/toyota/interface.py

@@ -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:
+    elif candidate in [CAR.LEXUS_RX, CAR.LEXUS_RXH, CAR.LEXUS_RX_TSS2, CAR.LEXUS_RXH_TSS2]:
-      stop_and_go = True
-      ret.wheelbase = 2.79
-      ret.steerRatio = 14.8
-      tire_stiffness_factor = 0.5533
-      ret.mass = 4387. * CV.LB_TO_KG + STD_CARGO_KG
-      set_lat_tune(ret.lateralTuning, LatTunes.PID_B)
-
-    elif candidate == CAR.LEXUS_RXH:
       stop_and_go = True
       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):
-
+    hud_control = c.hudControl
-    can_sends = self.CC.update(c.enabled, c.active, self.CS, self.frame,
+    ret = self.CC.update(c.enabled, c.active, self.CS, self.frame,
                          c.actuators, c.cruiseControl.cancel,
-                               c.hudControl.visualAlert, c.hudControl.leftLaneVisible,
+                         hud_control.visualAlert, hud_control.leftLaneVisible,
-                               c.hudControl.rightLaneVisible, c.hudControl.leadVisible,
+                         hud_control.rightLaneVisible, hud_control.leadVisible,
-                               c.hudControl.leftLaneDepart, c.hudControl.rightLaneDepart)
+                         hud_control.leftLaneDepart, hud_control.rightLaneDepart)
 
     self.frame += 1
-    return can_sends
+    return ret

selfdrive/car/toyota/toyotacan.py

@@ -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,

selfdrive/car/toyota/tunes.py

@@ -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]

selfdrive/car/toyota/values.py

@@ -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}

selfdrive/car/volkswagen/carcontroller.py

@@ -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

selfdrive/car/volkswagen/interface.py

@@ -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,
+    hud_control = c.hudControl
-                   c.hudControl.visualAlert,
+    ret = self.CC.update(c.enabled, self.CS, self.frame, self.ext_bus, c.actuators,
-                   c.hudControl.leftLaneVisible,
+                         hud_control.visualAlert,
-                   c.hudControl.rightLaneVisible,
+                         hud_control.leftLaneVisible,
-                   c.hudControl.leftLaneDepart,
+                         hud_control.rightLaneVisible,
-                   c.hudControl.rightLaneDepart)
+                         hud_control.leftLaneDepart,
+                         hud_control.rightLaneDepart)
     self.frame += 1
-    return can_sends
+    return ret

selfdrive/car/volkswagen/values.py

@@ -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',

selfdrive/common/modeldata.h

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

selfdrive/common/params.cc

@@ -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},

selfdrive/common/util.cc

@@ -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()) {

selfdrive/common/util.h

@@ -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);

selfdrive/common/version.h

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

selfdrive/controls/controlsd.py

@@ -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:
+    orientation_value = self.sm['liveLocationKalman'].orientationNED.value
-      CC.roll = self.sm['liveLocationKalman'].orientationNED.value[0]
+    if len(orientation_value) > 2:
-      CC.pitch = self.sm['liveLocationKalman'].orientationNED.value[1]
+      CC.roll = orientation_value[0]
+      CC.pitch = orientation_value[1]
 
     CC.cruiseControl.cancel = CS.cruiseState.enabled and (not self.enabled or not self.CP.pcmCruise)
     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)
+    hudControl = CC.hudControl
-    CC.hudControl.speedVisible = self.enabled
+    hudControl.setSpeed = float(self.v_cruise_kph * CV.KPH_TO_MS)
-    CC.hudControl.lanesVisible = self.enabled
+    hudControl.speedVisible = self.enabled
-    CC.hudControl.leadVisible = self.sm['longitudinalPlan'].hasLead
+    hudControl.lanesVisible = self.enabled
+    hudControl.leadVisible = self.sm['longitudinalPlan'].hasLead
 
-    CC.hudControl.rightLaneVisible = True
+    hudControl.rightLaneVisible = True
-    CC.hudControl.leftLaneVisible = True
+    hudControl.leftLaneVisible = True
 
     recent_blinker = (self.sm.frame - self.last_blinker_frame) * DT_CTRL < 5.0  # 5s blinker cooldown
     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
+    model_v2 = self.sm['modelV2'] 
-    if len(meta.desirePrediction) and ldw_allowed:
+    desire_prediction = model_v2.meta.desirePrediction
+    if len(desire_prediction) and ldw_allowed:
       right_lane_visible = self.sm['lateralPlan'].rProb > 0.5
       left_lane_visible = self.sm['lateralPlan'].lProb > 0.5
-      l_lane_change_prob = meta.desirePrediction[Desire.laneChangeLeft - 1]
+      l_lane_change_prob = desire_prediction[Desire.laneChangeLeft - 1]
-      r_lane_change_prob = meta.desirePrediction[Desire.laneChangeRight - 1]
+      r_lane_change_prob = desire_prediction[Desire.laneChangeRight - 1]
-      l_lane_close = left_lane_visible and (self.sm['modelV2'].laneLines[1].y[0] > -(1.08 + CAMERA_OFFSET))
-      r_lane_close = right_lane_visible and (self.sm['modelV2'].laneLines[2].y[0] < (1.08 - CAMERA_OFFSET))
 
-      CC.hudControl.leftLaneDepart = bool(l_lane_change_prob > LANE_DEPARTURE_THRESHOLD and l_lane_close)
+      lane_lines = model_v2.laneLines
-      CC.hudControl.rightLaneDepart = bool(r_lane_change_prob > LANE_DEPARTURE_THRESHOLD and r_lane_close)
+      l_lane_close = left_lane_visible and (lane_lines[1].y[0] > -(1.08 + CAMERA_OFFSET))
+      r_lane_close = right_lane_visible and (lane_lines[2].y[0] < (1.08 - CAMERA_OFFSET))
 
-    if CC.hudControl.rightLaneDepart or CC.hudControl.leftLaneDepart:
+      hudControl.leftLaneDepart = bool(l_lane_change_prob > LANE_DEPARTURE_THRESHOLD and l_lane_close)
+      hudControl.rightLaneDepart = bool(r_lane_change_prob > LANE_DEPARTURE_THRESHOLD and r_lane_close)
+
+    if hudControl.rightLaneDepart or hudControl.leftLaneDepart:
       self.events.add(EventName.ldw)
 
     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

selfdrive/controls/lib/events.py

@@ -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.events: List[int] = []
-    self.static_events = []
+    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"),
   },

selfdrive/controls/lib/lane_planner.py

@@ -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:
+    lane_lines = md.laneLines
-      self.ll_t = (np.array(md.laneLines[1].t) + np.array(md.laneLines[2].t))/2
+    if len(lane_lines) == 4 and len(lane_lines[0].t) == TRAJECTORY_SIZE:
+      self.ll_t = (np.array(lane_lines[1].t) + np.array(lane_lines[2].t))/2
       # left and right ll x is the same
-      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.lll_y = np.array(lane_lines[1].y) - self.camera_offset
-      self.rll_y = np.array(md.laneLines[2].y) - self.camera_offset
+      self.rll_y = np.array(lane_lines[2].y) - self.camera_offset
       self.lll_prob = md.laneLineProbs[1]
       self.rll_prob = md.laneLineProbs[2]
       self.lll_std = md.laneLineStds[1]
       self.rll_std = md.laneLineStds[2]
 
-    if len(md.meta.desireState):
+    desire_state = md.meta.desireState
-      self.l_lane_change_prob = md.meta.desireState[log.LateralPlan.Desire.laneChangeLeft]
+    if len(desire_state):
-      self.r_lane_change_prob = md.meta.desireState[log.LateralPlan.Desire.laneChangeRight]
+      self.l_lane_change_prob = desire_state[log.LateralPlan.Desire.laneChangeLeft]
+      self.r_lane_change_prob = desire_state[log.LateralPlan.Desire.laneChangeRight]
 
   def get_d_path(self, v_ego, path_t, path_xyz):
     # Reduce reliance on lanelines that are too far apart or

selfdrive/controls/lib/latcontrol_angle.py

@@ -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,7 +18,7 @@ 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

selfdrive/controls/lib/latcontrol_indi.py

@@ -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:

selfdrive/controls/lib/latcontrol_lqr.py

@@ -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

selfdrive/controls/lib/latcontrol_pid.py

@@ -16,12 +16,12 @@ 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

selfdrive/controls/lib/lateral_mpc_lib/SConscript

@@ -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'])

selfdrive/controls/lib/lateral_planner.py

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

selfdrive/controls/lib/longcontrol.py

@@ -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,
+def long_control_state_trans(CP, active, long_control_state, v_ego, v_target_future,
-                             output_accel, brake_pressed, cruise_standstill, min_speed_can):
+                             output_accel, brake_pressed, cruise_standstill):
   """Update longitudinal control state machine"""
-  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
+                        (v_target_future < CP.vEgoStopping or brake_pressed))
-                         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:
+    speeds = long_plan.speeds
-      v_target_lower = interp(CP.longitudinalActuatorDelayLowerBound, T_IDXS[:CONTROL_N], long_plan.speeds)
+    if len(speeds) == CONTROL_N:
-      a_target_lower = 2 * (v_target_lower - long_plan.speeds[0])/CP.longitudinalActuatorDelayLowerBound - long_plan.accels[0]
+      v_target_lower = interp(CP.longitudinalActuatorDelayLowerBound, T_IDXS[:CONTROL_N], speeds)
+      a_target_lower = 2 * (v_target_lower - speeds[0])/CP.longitudinalActuatorDelayLowerBound - long_plan.accels[0]
 
-      v_target_upper = interp(CP.longitudinalActuatorDelayUpperBound, T_IDXS[:CONTROL_N], long_plan.speeds)
+      v_target_upper = interp(CP.longitudinalActuatorDelayUpperBound, T_IDXS[:CONTROL_N], speeds)
-      a_target_upper = 2 * (v_target_upper - long_plan.speeds[0])/CP.longitudinalActuatorDelayUpperBound - long_plan.accels[0]
+      a_target_upper = 2 * (v_target_upper - speeds[0])/CP.longitudinalActuatorDelayUpperBound - long_plan.accels[0]
       a_target = min(a_target_lower, a_target_upper)
 
-      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,
+                                                       v_target_future, output_accel,
-                                                       CS.brakePressed, CS.cruiseState.standstill, CP.minSpeedCan)
+                                                       CS.brakePressed, CS.cruiseState.standstill)
 
     if self.long_control_state == LongCtrlState.off or CS.gasPressed:
       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