Merge branch 'master' into disengage-on-gas

.github/workflows/selfdrive_tests.yaml

@@ -69,62 +69,62 @@ jobs:
                           rm -rf /tmp/scons_cache/* && \
                           scons -j$(nproc) --cache-populate"
 
-  build_mac:
+  #build_mac:
-    name: build macos
+  #  name: build macos
-    runs-on: macos-latest
+  #  runs-on: macos-latest
-    timeout-minutes: 60
+  #  timeout-minutes: 60
-    steps:
+  #  steps:
-    - uses: actions/checkout@v2
+  #  - uses: actions/checkout@v2
-      with:
+  #    with:
-        submodules: true
+  #      submodules: true
-    - name: Determine pre-existing Homebrew packages
+  #  - name: Determine pre-existing Homebrew packages
-      if: steps.dependency-cache.outputs.cache-hit != 'true'
+  #    if: steps.dependency-cache.outputs.cache-hit != 'true'
-      run: |
+  #    run: |
-        echo 'EXISTING_CELLAR<<EOF' >> $GITHUB_ENV
+  #      echo 'EXISTING_CELLAR<<EOF' >> $GITHUB_ENV
-        ls -1 /usr/local/Cellar >> $GITHUB_ENV
+  #      ls -1 /usr/local/Cellar >> $GITHUB_ENV
-        echo 'EOF' >> $GITHUB_ENV
+  #      echo 'EOF' >> $GITHUB_ENV
-    - name: Cache dependencies
+  #  - name: Cache dependencies
-      id: dependency-cache
+  #    id: dependency-cache
-      uses: actions/cache@v2
+  #    uses: actions/cache@v2
-      with:
+  #    with:
-        path: |
+  #      path: |
-          ~/.pyenv
+  #        ~/.pyenv
-          ~/.local/share/virtualenvs/
+  #        ~/.local/share/virtualenvs/
-          /usr/local/Cellar
+  #        /usr/local/Cellar
-          ~/github_brew_cache_entries.txt
+  #        ~/github_brew_cache_entries.txt
-          /tmp/scons_cache
+  #        /tmp/scons_cache
-        key: macos-${{ hashFiles('tools/mac_setup.sh', 'update_requirements.sh', 'Pipfile*') }}
+  #      key: macos-${{ hashFiles('tools/mac_setup.sh', 'update_requirements.sh', 'Pipfile*') }}
-        restore-keys: macos-
+  #      restore-keys: macos-
-    - name: Brew link restored dependencies
+  #  - name: Brew link restored dependencies
-      run: |
+  #    run: |
-        if [ -f ~/github_brew_cache_entries.txt ]; then
+  #      if [ -f ~/github_brew_cache_entries.txt ]; then
-          while read pkg; do
+  #        while read pkg; do
-            brew link --force "$pkg" # `--force` for keg-only packages
+  #          brew link --force "$pkg" # `--force` for keg-only packages
-          done < ~/github_brew_cache_entries.txt
+  #        done < ~/github_brew_cache_entries.txt
-        else
+  #      else
-          echo "Cache entries not found"
+  #        echo "Cache entries not found"
-        fi
+  #      fi
-    - name: Install dependencies
+  #  - name: Install dependencies
-      run: ./tools/mac_setup.sh
+  #    run: ./tools/mac_setup.sh
-    - name: Build openpilot
+  #  - name: Build openpilot
-      run: |
+  #    run: |
-        source tools/openpilot_env.sh
+  #      source tools/openpilot_env.sh
-        pipenv run selfdrive/manager/build.py
+  #      pipenv run selfdrive/manager/build.py
-
+  #
-        # cleanup scons cache
+  #      # cleanup scons cache
-        rm -rf /tmp/scons_cache/
+  #      rm -rf /tmp/scons_cache/
-        pipenv run scons -j$(nproc) --cache-populate
+  #      pipenv run scons -j$(nproc) --cache-populate
-    - name: Remove pre-existing Homebrew packages for caching
+  #  - name: Remove pre-existing Homebrew packages for caching
-      if: steps.dependency-cache.outputs.cache-hit != 'true'
+  #    if: steps.dependency-cache.outputs.cache-hit != 'true'
-      run: |
+  #    run: |
-        cd /usr/local/Cellar
+  #      cd /usr/local/Cellar
-        new_cellar=$(ls -1)
+  #      new_cellar=$(ls -1)
-        comm -12 <(echo "$EXISTING_CELLAR") <(echo "$new_cellar") | while read pkg; do
+  #      comm -12 <(echo "$EXISTING_CELLAR") <(echo "$new_cellar") | while read pkg; do
-          if [[ $pkg != "zstd" ]]; then # caching step needs zstd
+  #        if [[ $pkg != "zstd" ]]; then # caching step needs zstd
-            rm -rf "$pkg"
+  #          rm -rf "$pkg"
-          fi
+  #        fi
-        done
+  #      done
-        comm -13 <(echo "$EXISTING_CELLAR") <(echo "$new_cellar") | tee ~/github_brew_cache_entries.txt
+  #      comm -13 <(echo "$EXISTING_CELLAR") <(echo "$new_cellar") | tee ~/github_brew_cache_entries.txt
 
   build_webcam:
     name: build webcam
@@ -264,9 +264,10 @@ jobs:
                         ./selfdrive/loggerd/tests/test_logger &&\
                         ./selfdrive/proclogd/tests/test_proclog && \
                         ./selfdrive/ui/replay/tests/test_replay && \
-                        ./selfdrive/camerad/test/ae_gray_test"
+                        ./selfdrive/camerad/test/ae_gray_test && \
-    - name: Upload coverage to Codecov
+                        coverage xml"
-      run: bash <(curl -s https://codecov.io/bash) -v -F unit_tests
+    - name: "Upload coverage to Codecov"
+      uses: codecov/codecov-action@v2
 
   process_replay:
     name: process replay
@@ -299,9 +300,10 @@ jobs:
     - name: Run replay
       run: |
         ${{ env.RUN }} "scons -j$(nproc) && \
-                        FILEREADER_CACHE=1 CI=1 coverage run selfdrive/test/process_replay/test_processes.py"
+                        FILEREADER_CACHE=1 CI=1 coverage run selfdrive/test/process_replay/test_processes.py && \
-    - name: Upload coverage to Codecov
+                        coverage xml"
-      run: bash <(curl -s https://codecov.io/bash) -v -F process_replay
+    - name: "Upload coverage to Codecov"
+      uses: codecov/codecov-action@v2
     - name: Print diff
       if: always()
       run: cat selfdrive/test/process_replay/diff.txt
@@ -354,7 +356,10 @@ jobs:
         ${{ env.RUN }} "mkdir -p selfdrive/test/out && \
                         scons -j$(nproc) && \
                         cd selfdrive/test/longitudinal_maneuvers && \
-                        ./test_longitudinal.py"
+                        coverage run ./test_longitudinal.py && \
+                        coverage xml"
+    - name: "Upload coverage to Codecov"
+      uses: codecov/codecov-action@v2
     - uses: actions/upload-artifact@v2
       if: always()
       continue-on-error: true
@@ -397,13 +402,14 @@ jobs:
     - name: Test car models
       run: |
         ${{ env.RUN }} "scons -j$(nproc) --test && \
-                        FILEREADER_CACHE=1 pytest selfdrive/test/test_models.py && \
+                        FILEREADER_CACHE=1 coverage run -m pytest selfdrive/test/test_models.py && \
+                        coverage xml && \
                         chmod -R 777 /tmp/comma_download_cache"
       env:
         NUM_JOBS: 4
         JOB_ID: ${{ matrix.job }}
-    - name: Upload coverage to Codecov
+    - name: "Upload coverage to Codecov"
-      run: bash <(curl -s https://codecov.io/bash) -v -F test_car_models
+      uses: codecov/codecov-action@v2
 
   docs:
     name: build docs

Jenkinsfile

@@ -157,6 +157,7 @@ pipeline {
                 }
                 */
 
+                /*
                 stage('C3: build') {
                   environment {
                     R3_PUSH = "${env.BRANCH_NAME == 'master' ? '1' : ' '}"
@@ -171,6 +172,7 @@ pipeline {
                     ])
                   }
                 }
+                */
 
                 stage('C3: HW + Unit Tests') {
                   steps {

RELEASES.md

@@ -1,12 +1,24 @@
-Version 0.8.13 (2022-XX-XX)
+Version 0.8.13 (2022-02-16)
 ========================
  * Improved driver monitoring
- * Roll compensation
+   * Retuned driver pose learner for relaxed driving positions
+   * Added reliance on driving model to be more scene adaptive
+   * Matched strictness between comma two and comma three
+ * Improved performance in turns by compensating for the road bank angle
  * Improved camera focus on the comma two
+ * AGNOS 4
+   * ADB support
+   * improved cell auto configuration
+ * NEOS 19
+   * package updates
+   * stability improvements
  * Subaru ECU firmware fingerprinting thanks to martinl!
  * Hyundai Santa Fe Plug-in Hybrid 2022 support thanks to sunnyhaibin!
+ * Mazda CX-5 2022 support thanks to Jafaral!
  * Subaru Impreza 2020 support thanks to martinl!
  * Toyota Avalon 2022 support thanks to sshane!
+ * Toyota Prius v 2017 support thanks to CT921!
+ * Volkswagen Caravelle 2020 support thanks to jyoung8607!
 
 Version 0.8.12 (2021-12-15)
 ========================

cereal

@@ -1 +1 @@
-Subproject commit b8ca92c1cd9b4448df09671f60c515460010bc78
+Subproject commit 03860ae0b2b8128cae7768e4301d889e627c9275

common/numpy_fast.py

@@ -1,6 +1,3 @@
-def int_rnd(x):
-  return int(round(x))
-
 def clip(x, lo, hi):
   return max(lo, min(hi, x))
 

docs/CARS.md

@@ -43,7 +43,7 @@
 | Lexus     | RX Hybrid 2016-19             | All               | Stock<sup>3</sup>| 0mph               | 0mph              |
 | Lexus     | RX Hybrid 2020-21             | All               | openpilot        | 0mph               | 0mph              |
 | Lexus     | UX Hybrid 2019-21             | All               | openpilot        | 0mph               | 0mph              |
-| Toyota    | Alphard 2020                  | All               | openpilot        | 0mph               | 0mph              |
+| Toyota    | Alphard 2019-20               | All               | openpilot        | 0mph               | 0mph              |
 | Toyota    | Avalon 2016-21                | TSS-P             | Stock<sup>3</sup>| 20mph<sup>1</sup>  | 0mph              |
 | Toyota    | Avalon 2022                   | All               | openpilot        | 0mph               | 0mph              |
 | Toyota    | Avalon Hybrid 2019-21         | TSS-P             | Stock<sup>3</sup>| 20mph<sup>1</sup>  | 0mph              |
@@ -64,6 +64,7 @@
 | Toyota    | Mirai 2021                    | All               | openpilot        | 0mph               | 0mph              |
 | Toyota    | Prius 2016-20                 | TSS-P             | Stock<sup>3</sup>| 0mph               | 0mph              |
 | Toyota    | Prius 2021-22                 | All               | openpilot        | 0mph               | 0mph              |
+| Toyota    | Prius v 2017                  | TSS-P             | Stock<sup>3</sup>| 20mph<sup>1</sup>  | 0mph              |
 | Toyota    | Prius Prime 2017-20           | All               | Stock<sup>3</sup>| 0mph               | 0mph              |
 | Toyota    | Prius Prime 2021-22           | All               | openpilot        | 0mph               | 0mph              |
 | Toyota    | Rav4 2016-18                  | TSS-P             | Stock<sup>3</sup>| 20mph<sup>1</sup>  | 0mph              |
@@ -86,10 +87,7 @@
 | Audi      | Q2 2018                         | ACC + Lane Assist | Stock            | 0mph               | 0mph         |
 | Audi      | Q3 2020-21                      | ACC + Lane Assist | Stock            | 0mph               | 0mph         |
 | Audi      | S3 2015                         | ACC + Lane Assist | Stock            | 0mph               | 0mph         |
-| Buick     | Regal 2018<sup>1</sup>          | Adaptive Cruise   | openpilot        | 0mph               | 7mph         |
-| Cadillac  | ATS 2018<sup>1</sup>            | Adaptive Cruise   | openpilot        | 0mph               | 7mph         |
 | Cadillac  | Escalade ESV 2016<sup>1</sup>   | ACC + LKAS        | openpilot        | 0mph               | 7mph         |
-| Chevrolet | Malibu 2017<sup>1</sup>         | Adaptive Cruise   | openpilot        | 0mph               | 7mph         |
 | Chevrolet | Volt 2017-18<sup>1</sup>        | Adaptive Cruise   | openpilot        | 0mph               | 7mph         |
 | Chrysler  | Pacifica 2017-18                | Adaptive Cruise   | Stock            | 0mph               | 9mph         |
 | Chrysler  | Pacifica 2020                   | Adaptive Cruise   | Stock            | 0mph               | 39mph        |
@@ -100,7 +98,6 @@
 | Genesis   | G80 2018                        | All               | Stock            | 0mph               | 0mph         |
 | Genesis   | G90 2018                        | All               | Stock            | 0mph               | 0mph         |
 | GMC       | Acadia 2018<sup>1</sup>         | Adaptive Cruise   | openpilot        | 0mph               | 7mph         |
-| Holden    | Astra 2017<sup>1</sup>          | Adaptive Cruise   | openpilot        | 0mph               | 7mph         |
 | Hyundai   | Elantra 2017-19                 | SCC + LKAS        | Stock            | 19mph              | 34mph        |
 | Hyundai   | Elantra 2021                    | SCC + LKAS        | Stock            | 0mph               | 0mph         |
 | Hyundai   | Elantra Hybrid 2021             | SCC + LKAS        | Stock            | 0mph               | 0mph         |
@@ -134,6 +131,7 @@
 | Kia       | Sorento 2018-19                 | SCC + LKAS        | Stock            | 0mph               | 0mph         |
 | Kia       | Stinger 2018                    | SCC + LKAS        | Stock            | 0mph               | 0mph         |
 | Kia       | Telluride 2020                  | SCC + LKAS        | Stock            | 0mph               | 0mph         |
+| Mazda     | CX-5 2022                       | All               | Stock            | 0mph               | 0mph         |
 | Mazda     | CX-9 2021                       | All               | Stock            | 0mph               | 28mph        |
 | Nissan    | Altima 2019-20                  | ProPILOT          | Stock            | 0mph               | 0mph         |
 | Nissan    | Leaf 2018-22                    | ProPILOT          | Stock            | 0mph               | 0mph         |
@@ -141,6 +139,10 @@
 | Nissan    | X-Trail 2017                    | ProPILOT          | Stock            | 0mph               | 0mph         |
 | SEAT      | Ateca 2018                      | Driver Assistance | Stock            | 0mph               | 0mph         |
 | SEAT      | Leon 2014-2020                  | Driver Assistance | Stock            | 0mph               | 0mph         |
+| Subaru    | Ascent 2019                     | EyeSight          | Stock            | 0mph               | 0mph         |
+| Subaru    | Crosstrek 2018-20               | EyeSight          | Stock            | 0mph               | 0mph         |
+| Subaru    | Forester 2019-21                | EyeSight          | Stock            | 0mph               | 0mph         |
+| Subaru    | Impreza 2017-19                 | EyeSight          | Stock            | 0mph               | 0mph         |
 | Škoda     | Kamiq 2021<sup>2</sup>          | Driver Assistance | Stock            | 0mph               | 0mph         |
 | Škoda     | Karoq 2019                      | Driver Assistance | Stock            | 0mph               | 0mph         |
 | Škoda     | Kodiaq 2018-19                  | Driver Assistance | Stock            | 0mph               | 0mph         |
@@ -148,12 +150,9 @@
 | Škoda     | Octavia RS 2016                 | Driver Assistance | Stock            | 0mph               | 0mph         |
 | Škoda     | Scala 2020                      | Driver Assistance | Stock            | 0mph               | 0mph         |
 | Škoda     | Superb 2015-18                  | Driver Assistance | Stock            | 0mph               | 0mph         |
-| Subaru    | Ascent 2019                     | EyeSight          | Stock            | 0mph               | 0mph         |
-| Subaru    | Crosstrek 2018-20               | EyeSight          | Stock            | 0mph               | 0mph         |
-| Subaru    | Forester 2019-21                | EyeSight          | Stock            | 0mph               | 0mph         |
-| Subaru    | Impreza 2017-19                 | EyeSight          | Stock            | 0mph               | 0mph         |
 | Volkswagen| Arteon 2018, 2021<sup>4</sup>   | Driver Assistance | Stock            | 0mph               | 0mph         |
 | Volkswagen| Atlas 2018-19, 2022<sup>4</sup> | Driver Assistance | Stock            | 0mph               | 0mph         |
+| Volkswagen| Caravelle 2020<sup>4</sup>      | Driver Assistance | Stock            | 0mph               | 32mph        |
 | Volkswagen| California 2021<sup>4</sup>     | Driver Assistance | Stock            | 0mph               | 32mph        |
 | Volkswagen| e-Golf 2014, 2019-20            | Driver Assistance | Stock            | 0mph               | 0mph         |
 | Volkswagen| Golf 2015-20                    | Driver Assistance | Stock            | 0mph               | 0mph         |

launch_env.sh

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

opendbc

@@ -1 +1 @@
-Subproject commit 2ffa8dfa4e2ccea4d81bdf5f0c62d496f64b53b1
+Subproject commit c3d3c71aa7a37364814f029778070fcb550c7cd3

panda

@@ -1 +1 @@
-Subproject commit a96615d25309fc29bad4eff782ef1c5aabd9124b
+Subproject commit f56ebf5b776b677bf12ec772b0223274dd798999

pyextra/acados_template/acados_ocp_solver_pyx.pyx

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

release/files_common

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

scripts/disable-powersave.py

@@ -0,0 +1,5 @@
+#!/usr/bin/env python3
+from selfdrive.hardware import HARDWARE
+
+if __name__ == "__main__":
+  HARDWARE.set_power_save(False)

selfdrive/athena/athenad.py

@@ -4,35 +4,39 @@ import hashlib
 import io
 import json
 import os
-import sys
 import queue
 import random
 import select
 import socket
+import subprocess
+import sys
+import tempfile
 import threading
 import time
-import tempfile
 from collections import namedtuple
+from datetime import datetime
 from functools import partial
-from typing import Any
+from typing import Any, Dict
 
 import requests
 from jsonrpc import JSONRPCResponseManager, dispatcher
-from websocket import ABNF, WebSocketTimeoutException, WebSocketException, create_connection
+from websocket import (ABNF, WebSocketException, WebSocketTimeoutException,
+                       create_connection)
 
 import cereal.messaging as messaging
+from cereal import log
 from cereal.services import service_list
 from common.api import Api
-from common.file_helpers import CallbackReader
 from common.basedir import PERSIST
+from common.file_helpers import CallbackReader
 from common.params import Params
 from common.realtime import sec_since_boot
-from selfdrive.hardware import HARDWARE, PC
+from selfdrive.hardware import HARDWARE, PC, TICI
 from selfdrive.loggerd.config import ROOT
 from selfdrive.loggerd.xattr_cache import getxattr, setxattr
-from selfdrive.swaglog import cloudlog, SWAGLOG_DIR
-from selfdrive.version import get_version, get_origin, get_short_branch, get_commit
 from selfdrive.statsd import STATS_DIR
+from selfdrive.swaglog import SWAGLOG_DIR, cloudlog
+from selfdrive.version import get_commit, get_origin, get_short_branch, get_version
 
 ATHENA_HOST = os.getenv('ATHENA_HOST', 'wss://athena.comma.ai')
 HANDLER_THREADS = int(os.getenv('HANDLER_THREADS', "4"))
@@ -44,8 +48,11 @@ RECONNECT_TIMEOUT_S = 70
 
 RETRY_DELAY = 10  # seconds
 MAX_RETRY_COUNT = 30  # Try for at most 5 minutes if upload fails immediately
+MAX_AGE = 31 * 24 * 3600  # seconds
 WS_FRAME_SIZE = 4096
 
+NetworkType = log.DeviceState.NetworkType
+
 dispatcher["echo"] = lambda s: s
 recv_queue: Any = queue.Queue()
 send_queue: Any = queue.Queue()
@@ -53,9 +60,12 @@ upload_queue: Any = queue.Queue()
 low_priority_send_queue: Any = queue.Queue()
 log_recv_queue: Any = queue.Queue()
 cancelled_uploads: Any = set()
-UploadItem = namedtuple('UploadItem', ['path', 'url', 'headers', 'created_at', 'id', 'retry_count', 'current', 'progress'], defaults=(0, False, 0))
+UploadItem = namedtuple('UploadItem', ['path', 'url', 'headers', 'created_at', 'id', 'retry_count', 'current', 'progress', 'allow_cellular'], defaults=(0, False, 0, False))
+
+cur_upload_items: Dict[int, Any] = {}
 
-cur_upload_items = {}
+class AbortTransferException(Exception):
+  pass
 
 
 class UploadQueueCache():
@@ -128,7 +138,29 @@ def jsonrpc_handler(end_event):
       send_queue.put_nowait(json.dumps({"error": str(e)}))
 
 
-def upload_handler(end_event):
+def retry_upload(tid: int, end_event: threading.Event, increase_count: bool = True) -> None:
+  if cur_upload_items[tid].retry_count < MAX_RETRY_COUNT:
+    item = cur_upload_items[tid]
+    new_retry_count = item.retry_count + 1 if increase_count else item.retry_count
+
+    item = item._replace(
+      retry_count=new_retry_count,
+      progress=0,
+      current=False
+    )
+    upload_queue.put_nowait(item)
+    UploadQueueCache.cache(upload_queue)
+
+    cur_upload_items[tid] = None
+
+    for _ in range(RETRY_DELAY):
+      time.sleep(1)
+      if end_event.is_set():
+        break
+
+
+def upload_handler(end_event: threading.Event) -> None:
+  sm = messaging.SubMaster(['deviceState'])
   tid = threading.get_ident()
 
   while not end_event.is_set():
@@ -141,31 +173,53 @@ def upload_handler(end_event):
         cancelled_uploads.remove(cur_upload_items[tid].id)
         continue
 
+      # Remove item if too old
+      age = datetime.now() - datetime.fromtimestamp(cur_upload_items[tid].created_at / 1000)
+      if age.total_seconds() > MAX_AGE:
+        cloudlog.event("athena.upload_handler.expired", item=cur_upload_items[tid], error=True)
+        continue
+
+      # Check if uploading over cell is allowed
+      sm.update(0)
+      cell = sm['deviceState'].networkType not in [NetworkType.wifi, NetworkType.ethernet]
+      if cell and (not cur_upload_items[tid].allow_cellular):
+        retry_upload(tid, end_event, False)
+        continue
+
       try:
         def cb(sz, cur):
+          # Abort transfer if connection changed to cell after starting upload
+          sm.update(0)
+          cell = sm['deviceState'].networkType not in [NetworkType.wifi, NetworkType.ethernet]
+          if cell and (not cur_upload_items[tid].allow_cellular):
+            raise AbortTransferException
+
           cur_upload_items[tid] = cur_upload_items[tid]._replace(progress=cur / sz if sz else 1)
 
-        _do_upload(cur_upload_items[tid], cb)
+
+        network_type = sm['deviceState'].networkType.raw
+        fn = cur_upload_items[tid].path
+        try:
+          sz = os.path.getsize(fn)
+        except OSError:
+          sz = -1
+
+        cloudlog.event("athena.upload_handler.upload_start", fn=fn, sz=sz, network_type=network_type)
+        response = _do_upload(cur_upload_items[tid], cb)
+
+        if response.status_code not in (200, 201, 403, 412):
+          cloudlog.event("athena.upload_handler.retry", status_code=response.status_code, fn=fn, sz=sz, network_type=network_type)
+          retry_upload(tid, end_event)
+        else:
+          cloudlog.event("athena.upload_handler.success", fn=fn, sz=sz, network_type=network_type)
+
         UploadQueueCache.cache(upload_queue)
-      except (requests.exceptions.Timeout, requests.exceptions.ConnectionError, requests.exceptions.SSLError) as e:
+      except (requests.exceptions.Timeout, requests.exceptions.ConnectionError, requests.exceptions.SSLError):
-        cloudlog.warning(f"athena.upload_handler.retry {e} {cur_upload_items[tid]}")
+        cloudlog.event("athena.upload_handler.timeout", fn=fn, sz=sz, network_type=network_type)
-
+        retry_upload(tid, end_event)
-        if cur_upload_items[tid].retry_count < MAX_RETRY_COUNT:
+      except AbortTransferException:
-          item = cur_upload_items[tid]
+        cloudlog.event("athena.upload_handler.abort", fn=fn, sz=sz, network_type=network_type)
-          item = item._replace(
+        retry_upload(tid, end_event, False)
-            retry_count=item.retry_count + 1,
-            progress=0,
-            current=False
-          )
-          upload_queue.put_nowait(item)
-          UploadQueueCache.cache(upload_queue)
-
-          cur_upload_items[tid] = None
-
-          for _ in range(RETRY_DELAY):
-            time.sleep(1)
-            if end_event.is_set():
-              break
 
     except queue.Empty:
       pass
@@ -266,15 +320,20 @@ def reboot():
 
 @dispatcher.add_method
 def uploadFileToUrl(fn, url, headers):
-  return uploadFilesToUrls([[fn, url, headers]])
+  return uploadFilesToUrls([{
+    "fn": fn,
+    "url": url,
+    "headers": headers,
+  }])
 
 
 @dispatcher.add_method
 def uploadFilesToUrls(files_data):
   items = []
   failed = []
-  for fn, url, headers in files_data:
+  for file in files_data:
-    if len(fn) == 0 or fn[0] == '/' or '..' in fn:
+    fn = file.get('fn', '')
+    if len(fn) == 0 or fn[0] == '/' or '..' in fn or 'url' not in file:
       failed.append(fn)
       continue
     path = os.path.join(ROOT, fn)
@@ -282,7 +341,14 @@ def uploadFilesToUrls(files_data):
       failed.append(fn)
       continue
 
-    item = UploadItem(path=path, url=url, headers=headers, created_at=int(time.time() * 1000), id=None)
+    item = UploadItem(
+      path=path,
+      url=file['url'],
+      headers=file.get('headers', {}),
+      created_at=int(time.time() * 1000),
+      id=None,
+      allow_cellular=file.get('allow_cellular', False),
+    )
     upload_id = hashlib.sha1(str(item).encode()).hexdigest()
     item = item._replace(id=upload_id)
     upload_queue.put_nowait(item)
@@ -322,6 +388,18 @@ def primeActivated(activated):
   return {"success": 1}
 
 
+@dispatcher.add_method
+def setBandwithLimit(upload_speed_kbps, download_speed_kbps):
+  if not TICI:
+    return {"success": 0, "error": "only supported on comma three"}
+
+  try:
+    HARDWARE.set_bandwidth_limit(upload_speed_kbps, download_speed_kbps)
+    return {"success": 1}
+  except subprocess.CalledProcessError as e:
+    return {"success": 0, "error": "failed to set limit", "stdout": e.stdout, "stderr": e.stderr}
+
+
 def startLocalProxy(global_end_event, remote_ws_uri, local_port):
   try:
     if local_port not in LOCAL_PORT_WHITELIST:
@@ -386,7 +464,7 @@ def getNetworks():
 
 @dispatcher.add_method
 def takeSnapshot():
-  from selfdrive.camerad.snapshot.snapshot import snapshot, jpeg_write
+  from selfdrive.camerad.snapshot.snapshot import jpeg_write, snapshot
   ret = snapshot()
   if ret is not None:
     def b64jpeg(x):

selfdrive/athena/registration.py

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

selfdrive/athena/tests/test_athenad.py

@@ -8,6 +8,7 @@ import time
 import threading
 import queue
 import unittest
+from datetime import datetime, timedelta
 
 from multiprocessing import Process
 from pathlib import Path
@@ -150,7 +151,7 @@ class TestAthenadMethods(unittest.TestCase):
   def test_upload_handler(self, host):
     fn = os.path.join(athenad.ROOT, 'qlog.bz2')
     Path(fn).touch()
-    item = athenad.UploadItem(path=fn, url=f"{host}/qlog.bz2", headers={}, created_at=int(time.time()*1000), id='')
+    item = athenad.UploadItem(path=fn, url=f"{host}/qlog.bz2", headers={}, created_at=int(time.time()*1000), id='', allow_cellular=True)
 
     end_event = threading.Event()
     thread = threading.Thread(target=athenad.upload_handler, args=(end_event,))
@@ -166,11 +167,36 @@ class TestAthenadMethods(unittest.TestCase):
     finally:
       end_event.set()
 
+  @with_http_server
+  @mock.patch('requests.put')
+  def test_upload_handler_retry(self, host, mock_put):
+    for status, retry in ((500, True), (412, False)):
+      mock_put.return_value.status_code = status
+      fn = os.path.join(athenad.ROOT, 'qlog.bz2')
+      Path(fn).touch()
+      item = athenad.UploadItem(path=fn, url=f"{host}/qlog.bz2", headers={}, created_at=int(time.time()*1000), id='', allow_cellular=True)
+
+      end_event = threading.Event()
+      thread = threading.Thread(target=athenad.upload_handler, args=(end_event,))
+      thread.start()
+
+      athenad.upload_queue.put_nowait(item)
+      try:
+        self.wait_for_upload()
+        time.sleep(0.1)
+
+        self.assertEqual(athenad.upload_queue.qsize(), 1 if retry else 0)
+      finally:
+        end_event.set()
+
+      if retry:
+        self.assertEqual(athenad.upload_queue.get().retry_count, 1)
+
   def test_upload_handler_timeout(self):
     """When an upload times out or fails to connect it should be placed back in the queue"""
     fn = os.path.join(athenad.ROOT, 'qlog.bz2')
     Path(fn).touch()
-    item = athenad.UploadItem(path=fn, url="http://localhost:44444/qlog.bz2", headers={}, created_at=int(time.time()*1000), id='')
+    item = athenad.UploadItem(path=fn, url="http://localhost:44444/qlog.bz2", headers={}, created_at=int(time.time()*1000), id='', allow_cellular=True)
     item_no_retry = item._replace(retry_count=MAX_RETRY_COUNT)
 
     end_event = threading.Event()
@@ -197,7 +223,7 @@ class TestAthenadMethods(unittest.TestCase):
       end_event.set()
 
   def test_cancelUpload(self):
-    item = athenad.UploadItem(path="qlog.bz2", url="http://localhost:44444/qlog.bz2", headers={}, created_at=int(time.time()*1000), id='id')
+    item = athenad.UploadItem(path="qlog.bz2", url="http://localhost:44444/qlog.bz2", headers={}, created_at=int(time.time()*1000), id='id', allow_cellular=True)
     athenad.upload_queue.put_nowait(item)
     dispatcher["cancelUpload"](item.id)
 
@@ -215,6 +241,28 @@ class TestAthenadMethods(unittest.TestCase):
     finally:
       end_event.set()
 
+  def test_cancelExpiry(self):
+    t_future = datetime.now() - timedelta(days=40)
+    ts = int(t_future.strftime("%s")) * 1000
+
+    # Item that would time out if actually uploaded
+    fn = os.path.join(athenad.ROOT, 'qlog.bz2')
+    Path(fn).touch()
+    item = athenad.UploadItem(path=fn, url="http://localhost:44444/qlog.bz2", headers={}, created_at=ts, id='', allow_cellular=True)
+
+
+    end_event = threading.Event()
+    thread = threading.Thread(target=athenad.upload_handler, args=(end_event,))
+    thread.start()
+    try:
+      athenad.upload_queue.put_nowait(item)
+      self.wait_for_upload()
+      time.sleep(0.1)
+
+      self.assertEqual(athenad.upload_queue.qsize(), 0)
+    finally:
+      end_event.set()
+
   def test_listUploadQueueEmpty(self):
     items = dispatcher["listUploadQueue"]()
     self.assertEqual(len(items), 0)
@@ -223,7 +271,7 @@ class TestAthenadMethods(unittest.TestCase):
   def test_listUploadQueueCurrent(self, host):
     fn = os.path.join(athenad.ROOT, 'qlog.bz2')
     Path(fn).touch()
-    item = athenad.UploadItem(path=fn, url=f"{host}/qlog.bz2", headers={}, created_at=int(time.time()*1000), id='')
+    item = athenad.UploadItem(path=fn, url=f"{host}/qlog.bz2", headers={}, created_at=int(time.time()*1000), id='', allow_cellular=True)
 
     end_event = threading.Event()
     thread = threading.Thread(target=athenad.upload_handler, args=(end_event,))
@@ -241,7 +289,7 @@ class TestAthenadMethods(unittest.TestCase):
       end_event.set()
 
   def test_listUploadQueue(self):
-    item = athenad.UploadItem(path="qlog.bz2", url="http://localhost:44444/qlog.bz2", headers={}, created_at=int(time.time()*1000), id='id')
+    item = athenad.UploadItem(path="qlog.bz2", url="http://localhost:44444/qlog.bz2", headers={}, created_at=int(time.time()*1000), id='id', allow_cellular=True)
     athenad.upload_queue.put_nowait(item)
 
     items = dispatcher["listUploadQueue"]()

selfdrive/boardd/SConscript

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

selfdrive/boardd/boardd.cc

@@ -1,3 +1,5 @@
+#include "selfdrive/boardd/boardd.h"
+
 #include <sched.h>
 #include <sys/cdefs.h>
 #include <sys/resource.h>
@@ -27,7 +29,6 @@
 #include "selfdrive/common/util.h"
 #include "selfdrive/hardware/hw.h"
 
-#include "selfdrive/boardd/panda.h"
 #include "selfdrive/boardd/pigeon.h"
 
 // -- Multi-panda conventions --
@@ -160,7 +161,8 @@ bool safety_setter_thread(std::vector<Panda *> pandas) {
   int safety_param;
 
   auto safety_configs = car_params.getSafetyConfigs();
-  for (uint32_t i=0; i<pandas.size(); i++) {
+  uint16_t unsafe_mode = car_params.getUnsafeMode();
+  for (uint32_t i = 0; i < pandas.size(); i++) {
     auto panda = pandas[i];
 
     if (safety_configs.size() > i) {
@@ -292,7 +294,7 @@ void send_empty_panda_state(PubMaster *pm) {
   pm->send("pandaStates", msg);
 }
 
-bool send_panda_states(PubMaster *pm, const std::vector<Panda *> &pandas, bool spoofing_started) {
+std::optional<bool> send_panda_states(PubMaster *pm, const std::vector<Panda *> &pandas, bool spoofing_started) {
   bool ignition_local = false;
 
   // build msg
@@ -302,34 +304,39 @@ bool send_panda_states(PubMaster *pm, const std::vector<Panda *> &pandas, bool s
 
   std::vector<health_t> pandaStates;
   for (const auto& panda : pandas){
-    health_t pandaState = panda->get_state();
+    auto health_opt = panda->get_state();
+    if (!health_opt) {
+      return std::nullopt;
+    }
+
+    health_t health = *health_opt;
 
     if (spoofing_started) {
-      pandaState.ignition_line = 1;
+      health.ignition_line_pkt = 1;
     }
 
-    ignition_local |= ((pandaState.ignition_line != 0) || (pandaState.ignition_can != 0));
+    ignition_local |= ((health.ignition_line_pkt != 0) || (health.ignition_can_pkt != 0));
 
-    pandaStates.push_back(pandaState);
+    pandaStates.push_back(health);
   }
 
-  for (uint32_t i=0; i<pandas.size(); i++) {
+  for (uint32_t i = 0; i < pandas.size(); i++) {
     auto panda = pandas[i];
-    const auto &pandaState = pandaStates[i];
+    const auto &health = pandaStates[i];
 
     // Make sure CAN buses are live: safety_setter_thread does not work if Panda CAN are silent and there is only one other CAN node
-    if (pandaState.safety_model == (uint8_t)(cereal::CarParams::SafetyModel::SILENT)) {
+    if (health.safety_mode_pkt == (uint8_t)(cereal::CarParams::SafetyModel::SILENT)) {
       panda->set_safety_model(cereal::CarParams::SafetyModel::NO_OUTPUT);
     }
 
   #ifndef __x86_64__
     bool power_save_desired = !ignition_local && !pigeon_active;
-    if (pandaState.power_save_enabled != power_save_desired) {
+    if (health.power_save_enabled_pkt != power_save_desired) {
       panda->set_power_saving(power_save_desired);
     }
 
     // set safety mode to NO_OUTPUT when car is off. ELM327 is an alternative if we want to leverage athenad/connect
-    if (!ignition_local && (pandaState.safety_model != (uint8_t)(cereal::CarParams::SafetyModel::NO_OUTPUT))) {
+    if (!ignition_local && (health.safety_mode_pkt != (uint8_t)(cereal::CarParams::SafetyModel::NO_OUTPUT))) {
       panda->set_safety_model(cereal::CarParams::SafetyModel::NO_OUTPUT);
     }
   #endif
@@ -339,25 +346,27 @@ bool send_panda_states(PubMaster *pm, const std::vector<Panda *> &pandas, bool s
     }
 
     auto ps = pss[i];
-    ps.setUptime(pandaState.uptime);
+    ps.setUptime(health.uptime_pkt);
-    ps.setIgnitionLine(pandaState.ignition_line);
+    ps.setBlockedCnt(health.blocked_msg_cnt_pkt);
-    ps.setIgnitionCan(pandaState.ignition_can);
+    ps.setIgnitionLine(health.ignition_line_pkt);
-    ps.setControlsAllowed(pandaState.controls_allowed);
+    ps.setIgnitionCan(health.ignition_can_pkt);
-    ps.setGasInterceptorDetected(pandaState.gas_interceptor_detected);
+    ps.setControlsAllowed(health.controls_allowed_pkt);
-    ps.setCanRxErrs(pandaState.can_rx_errs);
+    ps.setGasInterceptorDetected(health.gas_interceptor_detected_pkt);
-    ps.setCanSendErrs(pandaState.can_send_errs);
+    ps.setCanRxErrs(health.can_rx_errs_pkt);
-    ps.setCanFwdErrs(pandaState.can_fwd_errs);
+    ps.setCanSendErrs(health.can_send_errs_pkt);
-    ps.setGmlanSendErrs(pandaState.gmlan_send_errs);
+    ps.setCanFwdErrs(health.can_fwd_errs_pkt);
+    ps.setGmlanSendErrs(health.gmlan_send_errs_pkt);
     ps.setPandaType(panda->hw_type);
-    ps.setSafetyModel(cereal::CarParams::SafetyModel(pandaState.safety_model));
+    ps.setSafetyModel(cereal::CarParams::SafetyModel(health.safety_mode_pkt));
-    ps.setSafetyParam(pandaState.safety_param);
+    ps.setSafetyParam(health.safety_param_pkt);
-    ps.setFaultStatus(cereal::PandaState::FaultStatus(pandaState.fault_status));
+    ps.setFaultStatus(cereal::PandaState::FaultStatus(health.fault_status_pkt));
-    ps.setPowerSaveEnabled((bool)(pandaState.power_save_enabled));
+    ps.setPowerSaveEnabled((bool)(health.power_save_enabled_pkt));
-    ps.setHeartbeatLost((bool)(pandaState.heartbeat_lost));
+    ps.setHeartbeatLost((bool)(health.heartbeat_lost_pkt));
-    ps.setHarnessStatus(cereal::PandaState::HarnessStatus(pandaState.car_harness_status));
+    ps.setUnsafeMode(health.unsafe_mode_pkt);
+    ps.setHarnessStatus(cereal::PandaState::HarnessStatus(health.car_harness_status_pkt));
 
     // Convert faults bitset to capnp list
-    std::bitset<sizeof(pandaState.faults) * 8> fault_bits(pandaState.faults);
+    std::bitset<sizeof(health.faults_pkt) * 8> fault_bits(health.faults_pkt);
     auto faults = ps.initFaults(fault_bits.count());
 
     size_t j = 0;
@@ -375,7 +384,12 @@ bool send_panda_states(PubMaster *pm, const std::vector<Panda *> &pandas, bool s
 }
 
 void send_peripheral_state(PubMaster *pm, Panda *panda) {
-  health_t pandaState = panda->get_state();
+  auto pandaState_opt = panda->get_state();
+  if (!pandaState_opt) {
+    return;
+  }
+
+  health_t pandaState = *pandaState_opt;
 
   // build msg
   MessageBuilder msg;
@@ -394,12 +408,12 @@ void send_peripheral_state(PubMaster *pm, Panda *panda) {
       LOGW("reading hwmon took %lfms", read_time);
     }
   } else {
-    ps.setVoltage(pandaState.voltage);
+    ps.setVoltage(pandaState.voltage_pkt);
-    ps.setCurrent(pandaState.current);
+    ps.setCurrent(pandaState.current_pkt);
   }
 
   uint16_t fan_speed_rpm = panda->get_fan_speed();
-  ps.setUsbPowerMode(cereal::PeripheralState::UsbPowerMode(pandaState.usb_power_mode));
+  ps.setUsbPowerMode(cereal::PeripheralState::UsbPowerMode(pandaState.usb_power_mode_pkt));
   ps.setFanSpeedRpm(fan_speed_rpm);
 
   pm->send("peripheralState", msg);
@@ -423,7 +437,13 @@ void panda_state_thread(PubMaster *pm, std::vector<Panda *> pandas, bool spoofin
 
     // send out peripheralState
     send_peripheral_state(pm, peripheral_panda);
-    ignition = send_panda_states(pm, pandas, spoofing_started);
+    auto ignition_opt = send_panda_states(pm, pandas, spoofing_started);
+
+    if (!ignition_opt) {
+      continue;
+    }
+
+    ignition = *ignition_opt;
 
     // TODO: make this check fast, currently takes 16ms
     // check if we have new pandas and are offroad
@@ -589,22 +609,11 @@ void pigeon_thread(Panda *panda) {
   }
 }
 
-int main(int argc, char *argv[]) {
+void boardd_main_thread(std::vector<std::string> serials) {
-  LOGW("starting boardd");
+  if (serials.size() == 0) serials.push_back("");
-
-  if (!Hardware::PC()) {
-    int err;
-    err = util::set_realtime_priority(54);
-    assert(err == 0);
-    err = util::set_core_affinity({Hardware::TICI() ? 4 : 3});
-    assert(err == 0);
-  }
 
-  LOGW("attempting to connect");
   PubMaster pm({"pandaStates", "peripheralState"});
-
+  LOGW("attempting to connect");
-  std::vector<std::string> serials(argv + 1, argv + argc);
-  if (serials.size() == 0) serials.push_back("");
 
   // connect to all provided serials
   std::vector<Panda *> pandas;

selfdrive/boardd/boardd.h

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

selfdrive/boardd/main.cc

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

selfdrive/boardd/panda.cc

@@ -45,11 +45,11 @@ Panda::Panda(std::string serial, uint32_t bus_offset) : bus_offset(bus_offset) {
     libusb_device_descriptor desc;
     libusb_get_device_descriptor(dev_list[i], &desc);
     if (desc.idVendor == 0xbbaa && desc.idProduct == 0xddcc) {
-      libusb_open(dev_list[i], &dev_handle);
+      int ret = libusb_open(dev_list[i], &dev_handle);
-      if (dev_handle == NULL) { goto fail; }
+      if (dev_handle == NULL || ret < 0) { goto fail; }
 
       unsigned char desc_serial[26] = { 0 };
-      int ret = libusb_get_string_descriptor_ascii(dev_handle, desc.iSerialNumber, desc_serial, std::size(desc_serial));
+      ret = libusb_get_string_descriptor_ascii(dev_handle, desc.iSerialNumber, desc_serial, std::size(desc_serial));
       if (ret < 0) { goto fail; }
 
       usb_serial = std::string((char *)desc_serial, ret).c_str();
@@ -130,12 +130,14 @@ std::vector<std::string> Panda::list() {
     libusb_get_device_descriptor(device, &desc);
     if (desc.idVendor == 0xbbaa && desc.idProduct == 0xddcc) {
       libusb_device_handle *handle = NULL;
-      libusb_open(device, &handle);
+      int ret = libusb_open(device, &handle);
+      if (ret < 0) { goto finish; }
+
       unsigned char desc_serial[26] = { 0 };
-      int ret = libusb_get_string_descriptor_ascii(handle, desc.iSerialNumber, desc_serial, std::size(desc_serial));
+      ret = libusb_get_string_descriptor_ascii(handle, desc.iSerialNumber, desc_serial, std::size(desc_serial));
       libusb_close(handle);
-
       if (ret < 0) { goto finish; }
+
       serials.push_back(std::string((char *)desc_serial, ret).c_str());
     }
   }
@@ -309,10 +311,10 @@ void Panda::set_ir_pwr(uint16_t ir_pwr) {
   usb_write(0xb0, ir_pwr, 0);
 }
 
-health_t Panda::get_state() {
+std::optional<health_t> Panda::get_state() {
   health_t health {0};
-  usb_read(0xd2, 0, 0, (unsigned char*)&health, sizeof(health));
+  int err = usb_read(0xd2, 0, 0, (unsigned char*)&health, sizeof(health));
-  return health;
+  return err >= 0 ? std::make_optional(health) : std::nullopt;
 }
 
 void Panda::set_loopback(bool loopback) {

selfdrive/boardd/panda.h

@@ -13,6 +13,7 @@
 
 #include "cereal/gen/cpp/car.capnp.h"
 #include "cereal/gen/cpp/log.capnp.h"
+#include "panda/board/health.h"
 
 #define TIMEOUT 0
 #define PANDA_BUS_CNT 4
@@ -24,29 +25,6 @@
 #define CANPACKET_REJECTED  (0xC0U)
 #define CANPACKET_RETURNED  (0x80U)
 
-// copied from panda/board/main.c
-struct __attribute__((packed)) health_t {
-  uint32_t uptime;
-  uint32_t voltage;
-  uint32_t current;
-  uint32_t can_rx_errs;
-  uint32_t can_send_errs;
-  uint32_t can_fwd_errs;
-  uint32_t gmlan_send_errs;
-  uint32_t faults;
-  uint8_t ignition_line;
-  uint8_t ignition_can;
-  uint8_t controls_allowed;
-  uint8_t gas_interceptor_detected;
-  uint8_t car_harness_status;
-  uint8_t usb_power_mode;
-  uint8_t safety_model;
-  int16_t safety_param;
-  uint8_t fault_status;
-  uint8_t power_save_enabled;
-  uint8_t heartbeat_lost;
-};
-
 struct __attribute__((packed)) can_header {
   uint8_t reserved : 1;
   uint8_t bus : 3;
@@ -102,7 +80,7 @@ class Panda {
   void set_fan_speed(uint16_t fan_speed);
   uint16_t get_fan_speed();
   void set_ir_pwr(uint16_t ir_pwr);
-  health_t get_state();
+  std::optional<health_t> get_state();
   void set_loopback(bool loopback);
   std::optional<std::vector<uint8_t>> get_firmware_version();
   std::optional<std::string> get_serial();

selfdrive/camerad/cameras/camera_common.cc

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

selfdrive/camerad/cameras/camera_common.h

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

selfdrive/camerad/cameras/camera_qcom2.cc

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

selfdrive/camerad/main.cc

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

selfdrive/camerad/test/ae_gray_test.cc

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

selfdrive/car/chrysler/carstate.py

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

selfdrive/car/chrysler/radar_interface.py

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

selfdrive/car/ford/carstate.py

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

selfdrive/car/ford/interface.py

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

selfdrive/car/ford/radar_interface.py

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

selfdrive/car/gm/carcontroller.py

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

selfdrive/car/gm/carstate.py

@@ -3,8 +3,7 @@ from common.numpy_fast import mean
 from opendbc.can.can_define import CANDefine
 from opendbc.can.parser import CANParser
 from selfdrive.car.interfaces import CarStateBase
-from selfdrive.car.gm.values import DBC, CAR, AccState, CanBus, \
+from selfdrive.car.gm.values import DBC, CAR, AccState, CanBus, STEER_THRESHOLD
-                                    CruiseButtons, STEER_THRESHOLD
 
 
 class CarState(CarStateBase):
@@ -31,10 +30,8 @@ class CarState(CarStateBase):
     ret.standstill = ret.vEgoRaw < 0.01
 
     ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(pt_cp.vl["ECMPRDNL"]["PRNDL"], None))
+    ret.brakePressed = pt_cp.vl["ECMEngineStatus"]["Brake_Pressed"] != 0
     ret.brake = pt_cp.vl["EBCMBrakePedalPosition"]["BrakePedalPosition"] / 0xd0
-    # Brake pedal's potentiometer returns near-zero reading even when pedal is not pressed.
-    if ret.brake < 10/0xd0:
-      ret.brake = 0.
 
     ret.gas = pt_cp.vl["AcceleratorPedal2"]["AcceleratorPedal2"] / 254.
     ret.gasPressed = ret.gas > 1e-5
@@ -63,14 +60,13 @@ class CarState(CarStateBase):
     ret.rightBlinker = pt_cp.vl["BCMTurnSignals"]["TurnSignals"] == 2
 
     self.park_brake = pt_cp.vl["EPBStatus"]["EPBClosed"]
-    ret.cruiseState.available = bool(pt_cp.vl["ECMEngineStatus"]["CruiseMainOn"])
+    ret.cruiseState.available = pt_cp.vl["ECMEngineStatus"]["CruiseMainOn"] != 0
     ret.espDisabled = pt_cp.vl["ESPStatus"]["TractionControlOn"] != 1
     self.pcm_acc_status = pt_cp.vl["AcceleratorPedal2"]["CruiseState"]
 
-    ret.brakePressed = ret.brake > 1e-5
     # Regen braking is braking
     if self.car_fingerprint == CAR.VOLT:
-      ret.brakePressed = ret.brakePressed or bool(pt_cp.vl["EBCMRegenPaddle"]["RegenPaddle"])
+      ret.brakePressed = ret.brakePressed or pt_cp.vl["EBCMRegenPaddle"]["RegenPaddle"] != 0
 
     ret.cruiseState.enabled = self.pcm_acc_status != AccState.OFF
     ret.cruiseState.standstill = self.pcm_acc_status == AccState.STANDSTILL
@@ -79,33 +75,33 @@ class CarState(CarStateBase):
 
   @staticmethod
   def get_can_parser(CP):
-    # this function generates lists for signal, messages and initial values
     signals = [
-      # sig_name, sig_address, default
+      # sig_name, sig_address
-      ("BrakePedalPosition", "EBCMBrakePedalPosition", 0),
+      ("BrakePedalPosition", "EBCMBrakePedalPosition"),
-      ("FrontLeftDoor", "BCMDoorBeltStatus", 0),
+      ("FrontLeftDoor", "BCMDoorBeltStatus"),
-      ("FrontRightDoor", "BCMDoorBeltStatus", 0),
+      ("FrontRightDoor", "BCMDoorBeltStatus"),
-      ("RearLeftDoor", "BCMDoorBeltStatus", 0),
+      ("RearLeftDoor", "BCMDoorBeltStatus"),
-      ("RearRightDoor", "BCMDoorBeltStatus", 0),
+      ("RearRightDoor", "BCMDoorBeltStatus"),
-      ("LeftSeatBelt", "BCMDoorBeltStatus", 0),
+      ("LeftSeatBelt", "BCMDoorBeltStatus"),
-      ("RightSeatBelt", "BCMDoorBeltStatus", 0),
+      ("RightSeatBelt", "BCMDoorBeltStatus"),
-      ("TurnSignals", "BCMTurnSignals", 0),
+      ("TurnSignals", "BCMTurnSignals"),
-      ("AcceleratorPedal2", "AcceleratorPedal2", 0),
+      ("AcceleratorPedal2", "AcceleratorPedal2"),
-      ("CruiseState", "AcceleratorPedal2", 0),
+      ("CruiseState", "AcceleratorPedal2"),
-      ("ACCButtons", "ASCMSteeringButton", CruiseButtons.UNPRESS),
+      ("ACCButtons", "ASCMSteeringButton"),
-      ("SteeringWheelAngle", "PSCMSteeringAngle", 0),
+      ("SteeringWheelAngle", "PSCMSteeringAngle"),
-      ("SteeringWheelRate", "PSCMSteeringAngle", 0),
+      ("SteeringWheelRate", "PSCMSteeringAngle"),
-      ("FLWheelSpd", "EBCMWheelSpdFront", 0),
+      ("FLWheelSpd", "EBCMWheelSpdFront"),
-      ("FRWheelSpd", "EBCMWheelSpdFront", 0),
+      ("FRWheelSpd", "EBCMWheelSpdFront"),
-      ("RLWheelSpd", "EBCMWheelSpdRear", 0),
+      ("RLWheelSpd", "EBCMWheelSpdRear"),
-      ("RRWheelSpd", "EBCMWheelSpdRear", 0),
+      ("RRWheelSpd", "EBCMWheelSpdRear"),
-      ("PRNDL", "ECMPRDNL", 0),
+      ("PRNDL", "ECMPRDNL"),
-      ("LKADriverAppldTrq", "PSCMStatus", 0),
+      ("LKADriverAppldTrq", "PSCMStatus"),
-      ("LKATorqueDelivered", "PSCMStatus", 0),
+      ("LKATorqueDelivered", "PSCMStatus"),
-      ("LKATorqueDeliveredStatus", "PSCMStatus", 0),
+      ("LKATorqueDeliveredStatus", "PSCMStatus"),
-      ("TractionControlOn", "ESPStatus", 0),
+      ("TractionControlOn", "ESPStatus"),
-      ("EPBClosed", "EPBStatus", 0),
+      ("EPBClosed", "EPBStatus"),
-      ("CruiseMainOn", "ECMEngineStatus", 0),
+      ("CruiseMainOn", "ECMEngineStatus"),
+      ("Brake_Pressed", "ECMEngineStatus"),
     ]
 
     checks = [
@@ -125,19 +121,15 @@ class CarState(CarStateBase):
     ]
 
     if CP.carFingerprint == CAR.VOLT:
-      signals += [
+      signals.append(("RegenPaddle", "EBCMRegenPaddle"))
-        ("RegenPaddle", "EBCMRegenPaddle", 0),
+      checks.append(("EBCMRegenPaddle", 50))
-      ]
-      checks += [
-        ("EBCMRegenPaddle", 50),
-      ]
 
     return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, CanBus.POWERTRAIN)
 
   @staticmethod
   def get_loopback_can_parser(CP):
     signals = [
-      ("RollingCounter", "ASCMLKASteeringCmd", 0),
+      ("RollingCounter", "ASCMLKASteeringCmd"),
     ]
 
     checks = [

selfdrive/car/gm/interface.py

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

selfdrive/car/gm/radar_interface.py

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

selfdrive/car/gm/values.py

@@ -113,11 +113,11 @@ FINGERPRINTS = {
 }
 
 DBC = {
-  CAR.HOLDEN_ASTRA: dbc_dict('gm_global_a_powertrain', 'gm_global_a_object', chassis_dbc='gm_global_a_chassis'),
+  CAR.HOLDEN_ASTRA: dbc_dict('gm_global_a_powertrain_generated', 'gm_global_a_object', chassis_dbc='gm_global_a_chassis'),
-  CAR.VOLT: dbc_dict('gm_global_a_powertrain', 'gm_global_a_object', chassis_dbc='gm_global_a_chassis'),
+  CAR.VOLT: dbc_dict('gm_global_a_powertrain_generated', 'gm_global_a_object', chassis_dbc='gm_global_a_chassis'),
-  CAR.MALIBU: dbc_dict('gm_global_a_powertrain', 'gm_global_a_object', chassis_dbc='gm_global_a_chassis'),
+  CAR.MALIBU: dbc_dict('gm_global_a_powertrain_generated', 'gm_global_a_object', chassis_dbc='gm_global_a_chassis'),
-  CAR.ACADIA: dbc_dict('gm_global_a_powertrain', 'gm_global_a_object', chassis_dbc='gm_global_a_chassis'),
+  CAR.ACADIA: dbc_dict('gm_global_a_powertrain_generated', 'gm_global_a_object', chassis_dbc='gm_global_a_chassis'),
-  CAR.CADILLAC_ATS: dbc_dict('gm_global_a_powertrain', 'gm_global_a_object', chassis_dbc='gm_global_a_chassis'),
+  CAR.CADILLAC_ATS: dbc_dict('gm_global_a_powertrain_generated', 'gm_global_a_object', chassis_dbc='gm_global_a_chassis'),
-  CAR.BUICK_REGAL: dbc_dict('gm_global_a_powertrain', 'gm_global_a_object', chassis_dbc='gm_global_a_chassis'),
+  CAR.BUICK_REGAL: dbc_dict('gm_global_a_powertrain_generated', 'gm_global_a_object', chassis_dbc='gm_global_a_chassis'),
-  CAR.ESCALADE_ESV: dbc_dict('gm_global_a_powertrain', 'gm_global_a_object', chassis_dbc='gm_global_a_chassis'),
+  CAR.ESCALADE_ESV: dbc_dict('gm_global_a_powertrain_generated', 'gm_global_a_object', chassis_dbc='gm_global_a_chassis'),
 }

selfdrive/car/honda/carcontroller.py

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

selfdrive/car/honda/carstate.py

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

selfdrive/car/honda/interface.py

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

selfdrive/car/honda/radar_interface.py

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

selfdrive/car/honda/values.py

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

selfdrive/car/hyundai/carcontroller.py

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

selfdrive/car/hyundai/carstate.py

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

selfdrive/car/hyundai/interface.py

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

selfdrive/car/hyundai/radar_interface.py

@@ -20,11 +20,11 @@ def get_radar_can_parser(CP):
   for addr in range(RADAR_START_ADDR, RADAR_START_ADDR + RADAR_MSG_COUNT):
     msg = f"RADAR_TRACK_{addr:x}"
     signals += [
-      ("STATE", msg, 0),
+      ("STATE", msg),
-      ("AZIMUTH", msg, 0),
+      ("AZIMUTH", msg),
-      ("LONG_DIST", msg, 0),
+      ("LONG_DIST", msg),
-      ("REL_ACCEL", msg, 0),
+      ("REL_ACCEL", msg),
-      ("REL_SPEED", msg, 0),
+      ("REL_SPEED", msg),
     ]
     checks += [(msg, 50)]
   return CANParser(DBC[CP.carFingerprint]['radar'], signals, checks, 1)
@@ -81,7 +81,7 @@ class RadarInterface(RadarInterfaceBase):
         self.pts[addr].dRel = math.cos(azimuth) * msg['LONG_DIST']
         self.pts[addr].yRel = 0.5 * -math.sin(azimuth) * msg['LONG_DIST']
         self.pts[addr].vRel = msg['REL_SPEED']
-        self.pts[addr].aRel = msg['REL_ACCEL'] 
+        self.pts[addr].aRel = msg['REL_ACCEL']
         self.pts[addr].yvRel = float('nan')
 
       else:

selfdrive/car/hyundai/values.py

@@ -463,22 +463,27 @@ FW_VERSIONS = {
       b'\xf1\x00TM ESC \x03 101 \x08\x02 58910-S2DA0',
       b'\xf1\x8758910-S2DA0\xf1\x00TM ESC \x03 101 \x08\x02 58910-S2DA0',
       b'\xf1\x8758910-S2GA0\xf1\x00TM ESC \x02 101 \x08\x04 58910-S2GA0',
+      b'\xf1\x8758910-S1DA0\xf1\x00TM ESC \x1e 102 \x08\x08 58910-S1DA0',
     ],
     (Ecu.engine, 0x7e0, None): [
       b'\xf1\x82TMBZN5TMD3XXXG2E',
       b'\xf1\x82TACVN5GSI3XXXH0A',
+      b'\xf1\x82TMCFD5MMCXXXXG0A',
     ],
     (Ecu.eps, 0x7d4, None): [
       b'\xf1\x00TM  MDPS C 1.00 1.02 56370-S2AA0 0B19',
+      b'\xf1\x00TM  MDPS C 1.00 1.01 56310-S1AB0 4TSDC101',
     ],
     (Ecu.fwdCamera, 0x7c4, None): [
       b'\xf1\x00TMA MFC  AT MEX LHD 1.00 1.01 99211-S2500 210205',
       b'\xf1\x00TMA MFC  AT USA LHD 1.00 1.00 99211-S2500 200720',
+      b'\xf1\x00TM  MFC  AT EUR LHD 1.00 1.03 99211-S1500 210224',
     ],
     (Ecu.transmission, 0x7e1, None): [
       b'\xf1\x00T02601BL  T02730A1  VTMPT25XXX730NS2\xa6\x06\x88\xf7',
       b'\xf1\x87SDMXCA8653204GN1EVugEUuWwwwwww\x87wwwwwv/\xfb\xff\xa8\x88\x9f\xff\xa5\x9c\xf1\x89HT6WAD00A1\xf1\x82STM4G25NH1\x00\x00\x00\x00\x00\x00',
       b'\xf1\x87954A02N250\x00\x00\x00\x00\x00\xf1\x81T02730A1  \xf1\x00T02601BL  T02730A1  VTMPT25XXX730NS2\xa6\x06\x88\xf7',
+      b'\xf1\x87KMMYBU034207SB72x\x89\x88\x98h\x88\x98\x89\x87fhvvfWf33_\xff\x87\xff\x8f\xfa\x81\xe5\xf1\x89HT6TAF00A1\xf1\x82STM0M25GS1\x00\x00\x00\x00\x00\x00',
     ],
   },
   CAR.SANTA_FE_HEV_2022: {

selfdrive/car/interfaces.py

@@ -74,8 +74,7 @@ class CarInterfaceBase(ABC):
   def get_std_params(candidate, fingerprint):
     ret = car.CarParams.new_message()
     ret.carFingerprint = candidate
-    ret.unsafeMode = 0  # see safety_declarations.h for allowed values
+    ret.unsafeMode = 1 if Params().get_bool("DisengageOnGas") else 0  # see safety_declarations.h for allowed values
-    ret.unsafeMode = 1 if Params().get_bool("DisengageOnGas") else 0 # sets unsafeMode to 1 if DisengageOnGas is True
 
     # standard ALC params
     ret.steerControlType = car.CarParams.SteerControlType.torque
@@ -101,6 +100,7 @@ class CarInterfaceBase(ABC):
     ret.longitudinalTuning.kiV = [1.]
     ret.longitudinalActuatorDelayLowerBound = 0.15
     ret.longitudinalActuatorDelayUpperBound = 0.15
+    ret.steerLimitTimer = 1.0
     return ret
 
   @abstractmethod

selfdrive/car/mazda/carcontroller.py

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

selfdrive/car/mazda/carstate.py

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

selfdrive/car/mazda/interface.py

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

selfdrive/car/mazda/values.py

@@ -19,7 +19,8 @@ class CAR:
   CX9 = "MAZDA CX-9"
   MAZDA3 = "MAZDA 3"
   MAZDA6 = "MAZDA 6"
-  CX9_2021 = "MAZDA CX-9 2021"   # No Steer Lockout
+  CX9_2021 = "MAZDA CX-9 2021"
+  CX5_2022 = "MAZDA CX-5 2022"
 
 class LKAS_LIMITS:
   STEER_THRESHOLD = 15
@@ -35,6 +36,26 @@ class Buttons:
 
 
 FW_VERSIONS = {
+  CAR.CX5_2022 : {
+    (Ecu.eps, 0x730, None): [
+      b'KSD5-3210X-C-00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
+    ],
+    (Ecu.engine, 0x7e0, None): [
+      b'PX2G-188K2-H\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
+    ],
+    (Ecu.fwdRadar, 0x764, None): [
+      b'K131-67XK2-F\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
+    ],
+    (Ecu.esp, 0x760, None): [
+      b'KSD5-437K2-A\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
+    ],
+    (Ecu.fwdCamera, 0x706, None): [
+      b'GSH7-67XK2-S\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
+    ],
+    (Ecu.transmission, 0x7e1, None): [
+      b'PYB2-21PS1-H\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
+    ],
+  },
   CAR.CX5: {
     (Ecu.eps, 0x730, None): [
       b'KJ01-3210X-G-00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
@@ -115,16 +136,19 @@ FW_VERSIONS = {
       b'PYD7-188K2-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
       b'PYD8-188K2-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
       b'PYFM-188K2-F\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
+      b'PYFM-188K2-H\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
     ],
     (Ecu.fwdRadar, 0x764, None): [
       b'K123-67XK2-F\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
       b'K131-67XK2-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
       b'K131-67XK2-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
       b'TK80-67XK2-E\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
+      b'TK80-67XK2-F\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
     ],
     (Ecu.esp, 0x760, None): [
       b'TA0B-437K2-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
       b'TK79-437K2-E\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
+      b'TK79-437K2-F\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
       b'TM53-437K2-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
       b'TN40-437K2-A\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
     ],
@@ -138,6 +162,7 @@ FW_VERSIONS = {
       b'PXM7-21PS1-A\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
       b'PXM7-21PS1-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
       b'PYFM-21PS1-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
+      b'PYFM-21PS1-D\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
       b'PYD5-21PS1-A\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
       b'PYD5-21PS1-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
       b'PYD6-21PS1-A\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
@@ -242,10 +267,8 @@ DBC = {
   CAR.MAZDA3: dbc_dict('mazda_2017', None),
   CAR.MAZDA6: dbc_dict('mazda_2017', None),
   CAR.CX9_2021: dbc_dict('mazda_2017', None),
+  CAR.CX5_2022: dbc_dict('mazda_2017', None),
 }
 
 # Gen 1 hardware: same CAN messages and same camera
-GEN1 = {CAR.CX5, CAR.CX9, CAR.CX9_2021, CAR.MAZDA3, CAR.MAZDA6}
+GEN1 = {CAR.CX5, CAR.CX9, CAR.CX9_2021, CAR.MAZDA3, CAR.MAZDA6, CAR.CX5_2022}
-
-# Cars with a steering lockout
-STEER_LOCKOUT_CAR = {CAR.CX5, CAR.CX9, CAR.MAZDA3, CAR.MAZDA6}

selfdrive/car/mock/interface.py

@@ -20,7 +20,6 @@ class CarInterface(CarInterfaceBase):
 
     cloudlog.debug("Using Mock Car Interface")
 
-    # TODO: subscribe to phone sensor
     self.sensor = messaging.sub_sock('sensorEvents')
     self.gps = messaging.sub_sock('gpsLocationExternal')
 

selfdrive/car/nissan/carcontroller.py

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

selfdrive/car/nissan/carstate.py

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

selfdrive/car/nissan/interface.py

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

selfdrive/car/subaru/carcontroller.py

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

selfdrive/car/subaru/carstate.py

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

selfdrive/car/subaru/interface.py

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

selfdrive/car/tesla/carcontroller.py

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

selfdrive/car/tesla/carstate.py

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

selfdrive/car/tesla/interface.py

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

selfdrive/car/tesla/radar_interface.py

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

selfdrive/car/toyota/carcontroller.py

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

selfdrive/car/toyota/carstate.py

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

selfdrive/car/toyota/interface.py

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

selfdrive/car/toyota/radar_interface.py

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

selfdrive/car/toyota/toyotacan.py

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

selfdrive/car/toyota/values.py

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

selfdrive/car/volkswagen/carcontroller.py

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

selfdrive/car/volkswagen/carstate.py

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

selfdrive/car/volkswagen/interface.py

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

selfdrive/car/volkswagen/values.py

@@ -165,6 +165,7 @@ FW_VERSIONS = {
       b'\xf1\x8704E906027HD\xf1\x893742',
       b'\xf1\x8704E906027MA\xf1\x894958',
       b'\xf1\x8704L906021DT\xf1\x895520',
+      b'\xf1\x8704L906021DT\xf1\x898127',
       b'\xf1\x8704L906021N \xf1\x895518',
       b'\xf1\x8704L906026BP\xf1\x897608',
       b'\xf1\x8704L906026NF\xf1\x899528',
@@ -211,6 +212,7 @@ FW_VERSIONS = {
       b'\xf1\x870D9300040A \xf1\x893613',
       b'\xf1\x870D9300040S \xf1\x894311',
       b'\xf1\x870D9300041H \xf1\x895220',
+      b'\xf1\x870D9300041P \xf1\x894507',
       b'\xf1\x870DD300045K \xf1\x891120',
       b'\xf1\x870DD300046F \xf1\x891601',
       b'\xf1\x870GC300012A \xf1\x891403',
@@ -469,13 +471,16 @@ FW_VERSIONS = {
   },
   CAR.TRANSPORTER_T61: {
     (Ecu.engine, 0x7e0, None): [
+      b'\xf1\x8704L906057AP\xf1\x891186',
       b'\xf1\x8704L906057N \xf1\x890413',
     ],
     (Ecu.transmission, 0x7e1, None): [
       b'\xf1\x870BT300012G \xf1\x893102',
+      b'\xf1\x870BT300012E \xf1\x893105',
     ],
     (Ecu.srs, 0x715, None): [
       b'\xf1\x872Q0959655AE\xf1\x890506\xf1\x82\02316170411110411--04041704161611152S1411',
+      b'\xf1\x872Q0959655AF\xf1\x890506\xf1\x82\x1316171111110411--04041711121211152S1413',
     ],
     (Ecu.eps, 0x712, None): [
       b'\xf1\x877LA909144F \xf1\x897150\xf1\x82\005323A5519A2',

selfdrive/common/SConscript

@@ -7,6 +7,7 @@ else:
 
 common_libs = [
   'params.cc',
+  'statlog.cc',
   'swaglog.cc',
   'util.cc',
   'gpio.cc',

selfdrive/common/clutil.cc

@@ -49,7 +49,7 @@ void cl_print_build_errors(cl_program program, cl_device_id device) {
   std::string log(log_size, '\0');
   clGetProgramBuildInfo(program, device, CL_PROGRAM_BUILD_LOG, log_size, &log[0], NULL);
 
-  std::cout << "build failed; status=" << status << ", log:" << std::endl << log << std::endl; 
+  std::cout << "build failed; status=" << status << ", log:" << std::endl << log << std::endl;
 }
 
 }  // namespace

selfdrive/common/params.cc

@@ -91,7 +91,6 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"CarParams", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_ON},
     {"CarParamsCache", CLEAR_ON_MANAGER_START},
     {"CarVin", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_ON},
-    {"CommunityFeaturesToggle", PERSISTENT},
     {"CompletedTrainingVersion", PERSISTENT},
     {"ControlsReady", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_ON},
     {"CurrentRoute", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_ON},
@@ -117,7 +116,6 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"GsmRoaming", PERSISTENT},
     {"HardwareSerial", PERSISTENT},
     {"HasAcceptedTerms", PERSISTENT},
-    {"HasPrime", PERSISTENT},
     {"IMEI", PERSISTENT},
     {"InstallDate", PERSISTENT},
     {"IsDriverViewEnabled", CLEAR_ON_MANAGER_START},
@@ -144,8 +142,10 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"NavdRender", PERSISTENT},
     {"OpenpilotEnabledToggle", PERSISTENT},
     {"PandaHeartbeatLost", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_OFF},
+    {"PandaSignatures", CLEAR_ON_MANAGER_START},
     {"Passive", PERSISTENT},
     {"PrimeRedirected", PERSISTENT},
+    {"PrimeType", PERSISTENT},
     {"RecordFront", PERSISTENT},
     {"RecordFrontLock", PERSISTENT},  // for the internal fleet
     {"ReleaseNotes", PERSISTENT},

selfdrive/common/statlog.cc

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

selfdrive/common/statlog.h

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

selfdrive/common/swaglog.cc

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

selfdrive/common/swaglog.h

@@ -8,6 +8,8 @@
 #define CLOUDLOG_ERROR 40
 #define CLOUDLOG_CRITICAL 50
 
+
+
 void cloudlog_e(int levelnum, const char* filename, int lineno, const char* func,
                 const char* fmt, ...) /*__attribute__ ((format (printf, 6, 7)))*/;
 

selfdrive/common/tests/test_swaglog.cc

@@ -12,29 +12,31 @@
 const char *SWAGLOG_ADDR = "ipc:///tmp/logmessage";
 std::string daemon_name = "testy";
 std::string dongle_id = "test_dongle_id";
+int LINE_NO = 0;
 
-void log_thread(int msg, int msg_cnt) {
+void log_thread(int thread_id, int msg_cnt) {
   for (int i = 0; i < msg_cnt; ++i) {
-    LOGD("%d", msg);
+    LOGD("%d", thread_id);
+    LINE_NO = __LINE__ - 1;
     usleep(1);
   }
 }
 
-void send_stop_msg(void *zctx) {
+void recv_log(int thread_cnt, int thread_msg_cnt) {
-  void *sock = zmq_socket(zctx, ZMQ_PUSH);
+  void *zctx = zmq_ctx_new();
-  zmq_connect(sock, SWAGLOG_ADDR);
-  zmq_send(sock, "", 0, ZMQ_NOBLOCK);
-  zmq_close(sock);
-}
-
-void recv_log(void *zctx, int thread_cnt, int thread_msg_cnt) {
   void *sock = zmq_socket(zctx, ZMQ_PULL);
   zmq_bind(sock, SWAGLOG_ADDR);
   std::vector<int> thread_msgs(thread_cnt);
+  int total_count = 0;
 
-  while (true) {
+  for (auto start = std::chrono::steady_clock::now(), now = start;
+       now < start + std::chrono::seconds{1} && total_count < (thread_cnt * thread_msg_cnt);
+       now = std::chrono::steady_clock::now()) {
     char buf[4096] = {};
-    if (zmq_recv(sock, buf, sizeof(buf), 0) == 0) break;
+    if (zmq_recv(sock, buf, sizeof(buf), ZMQ_DONTWAIT) <= 0) {
+      if (errno == EAGAIN || errno == EINTR || errno == EFSM) continue;
+      break;
+    }
 
     REQUIRE(buf[0] == CLOUDLOG_DEBUG);
     std::string err;
@@ -44,13 +46,16 @@ void recv_log(void *zctx, int thread_cnt, int thread_msg_cnt) {
     REQUIRE(msg["levelnum"].int_value() == CLOUDLOG_DEBUG);
     REQUIRE_THAT(msg["filename"].string_value(), Catch::Contains("test_swaglog.cc"));
     REQUIRE(msg["funcname"].string_value() == "log_thread");
-    REQUIRE(msg["lineno"].int_value() == 18);  // TODO: do this automatically
+    REQUIRE(msg["lineno"].int_value() == LINE_NO);
 
     auto ctx = msg["ctx"];
+
     REQUIRE(ctx["daemon"].string_value() == daemon_name);
     REQUIRE(ctx["dongle_id"].string_value() == dongle_id);
-    REQUIRE(ctx["version"].string_value() == COMMA_VERSION);
     REQUIRE(ctx["dirty"].bool_value() == true);
+
+    REQUIRE(ctx["version"].string_value() == COMMA_VERSION);
+
     std::string device = "pc";
     if (Hardware::EON()) {
       device = "eon";
@@ -62,11 +67,14 @@ void recv_log(void *zctx, int thread_cnt, int thread_msg_cnt) {
     int thread_id = atoi(msg["msg"].string_value().c_str());
     REQUIRE((thread_id >= 0 && thread_id < thread_cnt));
     thread_msgs[thread_id]++;
+    total_count++;
   }
   for (int i = 0; i < thread_cnt; ++i) {
+    INFO("thread :" << i);
     REQUIRE(thread_msgs[i] == thread_msg_cnt);
   }
   zmq_close(sock);
+  zmq_ctx_destroy(zctx);
 }
 
 TEST_CASE("swaglog") {
@@ -76,14 +84,11 @@ TEST_CASE("swaglog") {
   const int thread_cnt = 5;
   const int thread_msg_cnt = 100;
 
-  void *zctx = zmq_ctx_new();
-  send_stop_msg(zctx);
   std::vector<std::thread> log_threads;
   for (int i = 0; i < thread_cnt; ++i) {
     log_threads.push_back(std::thread(log_thread, i, thread_msg_cnt));
   }
-
   for (auto &t : log_threads) t.join();
-  recv_log(zctx, thread_cnt, thread_msg_cnt);
+
-  zmq_ctx_destroy(zctx);
+  recv_log(thread_cnt, thread_msg_cnt);
 }

selfdrive/common/util.h

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

selfdrive/controls/controlsd.py

@@ -31,8 +31,6 @@ from selfdrive.manager.process_config import managed_processes
 SOFT_DISABLE_TIME = 3  # seconds
 LDW_MIN_SPEED = 31 * CV.MPH_TO_MS
 LANE_DEPARTURE_THRESHOLD = 0.1
-STEER_ANGLE_SATURATION_TIMEOUT = 1.0 / DT_CTRL
-STEER_ANGLE_SATURATION_THRESHOLD = 2.5  # Degrees
 
 REPLAY = "REPLAY" in os.environ
 SIMULATION = "SIMULATION" in os.environ
@@ -100,7 +98,6 @@ class Controls:
     # read params
     self.is_metric = params.get_bool("IsMetric")
     self.is_ldw_enabled = params.get_bool("IsLdwEnabled")
-    community_feature_toggle = params.get_bool("CommunityFeaturesToggle")
     openpilot_enabled_toggle = params.get_bool("OpenpilotEnabledToggle")
     passive = params.get_bool("Passive") or not openpilot_enabled_toggle
 
@@ -110,11 +107,7 @@ class Controls:
     car_recognized = self.CP.carName != 'mock'
 
     controller_available = self.CI.CC is not None and not passive and not self.CP.dashcamOnly
-    community_feature = self.CP.communityFeature or \
+    self.read_only = not car_recognized or not controller_available or self.CP.dashcamOnly
-                        self.CP.fingerprintSource == car.CarParams.FingerprintSource.can
-    community_feature_disallowed = community_feature and (not community_feature_toggle)
-    self.read_only = not car_recognized or not controller_available or \
-                       self.CP.dashcamOnly or community_feature_disallowed
     if self.read_only:
       safety_config = car.CarParams.SafetyConfig.new_message()
       safety_config.safetyModel = car.CarParams.SafetyModel.noOutput
@@ -133,13 +126,13 @@ class Controls:
     self.VM = VehicleModel(self.CP)
 
     if self.CP.steerControlType == car.CarParams.SteerControlType.angle:
-      self.LaC = LatControlAngle(self.CP)
+      self.LaC = LatControlAngle(self.CP, self.CI)
     elif self.CP.lateralTuning.which() == 'pid':
       self.LaC = LatControlPID(self.CP, self.CI)
     elif self.CP.lateralTuning.which() == 'indi':
-      self.LaC = LatControlINDI(self.CP)
+      self.LaC = LatControlINDI(self.CP, self.CI)
     elif self.CP.lateralTuning.which() == 'lqr':
-      self.LaC = LatControlLQR(self.CP)
+      self.LaC = LatControlLQR(self.CP, self.CI)
 
     self.initialized = False
     self.state = State.disabled
@@ -153,7 +146,6 @@ class Controls:
     self.cruise_mismatch_counter = 0
     self.can_rcv_error_counter = 0
     self.last_blinker_frame = 0
-    self.saturated_count = 0
     self.distance_traveled = 0
     self.last_functional_fan_frame = 0
     self.events_prev = []
@@ -169,8 +161,6 @@ class Controls:
 
     if not sounds_available:
       self.events.add(EventName.soundsUnavailable, static=True)
-    if community_feature_disallowed and car_recognized and not self.CP.dashcamOnly:
-      self.events.add(EventName.communityFeatureDisallowed, static=True)
     if not car_recognized:
       self.events.add(EventName.carUnrecognized, static=True)
       if len(self.CP.carFw) > 0:
@@ -191,10 +181,8 @@ class Controls:
     """Compute carEvents from carState"""
 
     self.events.clear()
-    self.events.add_from_msg(CS.events)
-    self.events.add_from_msg(self.sm['driverMonitoringState'].events)
 
-    # Handle startup event
+    # Add startup event
     if self.startup_event is not None:
       self.events.add(self.startup_event)
       self.startup_event = None
@@ -204,6 +192,9 @@ class Controls:
       self.events.add(EventName.controlsInitializing)
       return
 
+    self.events.add_from_msg(CS.events)
+    self.events.add_from_msg(self.sm['driverMonitoringState'].events)
+
     # Create events for battery, temperature, disk space, and memory
     if EON and (self.sm['peripheralState'].pandaType != PandaType.uno) and \
        self.sm['deviceState'].batteryPercent < 1 and self.sm['deviceState'].chargingError:
@@ -260,9 +251,12 @@ class Controls:
     for i, pandaState in enumerate(self.sm['pandaStates']):
       # All pandas must match the list of safetyConfigs, and if outside this list, must be silent or noOutput
       if i < len(self.CP.safetyConfigs):
-        safety_mismatch = pandaState.safetyModel != self.CP.safetyConfigs[i].safetyModel or pandaState.safetyParam != self.CP.safetyConfigs[i].safetyParam
+        safety_mismatch = pandaState.safetyModel != self.CP.safetyConfigs[i].safetyModel or \
+                          pandaState.safetyParam != self.CP.safetyConfigs[i].safetyParam or \
+                          pandaState.unsafeMode != self.CP.unsafeMode
       else:
         safety_mismatch = pandaState.safetyModel not in IGNORED_SAFETY_MODES
+
       if safety_mismatch or self.mismatch_counter >= 200:
         self.events.add(EventName.controlsMismatch)
 
@@ -279,7 +273,7 @@ class Controls:
       if not self.logged_comm_issue:
         invalid = [s for s, valid in self.sm.valid.items() if not valid]
         not_alive = [s for s, alive in self.sm.alive.items() if not alive]
-        cloudlog.event("commIssue", invalid=invalid, not_alive=not_alive)
+        cloudlog.event("commIssue", invalid=invalid, not_alive=not_alive, can_error=self.can_rcv_error, error=True)
         self.logged_comm_issue = True
     else:
       self.logged_comm_issue = False
@@ -311,7 +305,7 @@ class Controls:
       self.events.add(EventName.fcw)
 
     if TICI:
-      for m in  messaging.drain_sock(self.log_sock, wait_for_one=False):
+      for m in messaging.drain_sock(self.log_sock, wait_for_one=False):
         try:
           msg = m.androidLog.message
           if any(err in msg for err in ("ERROR_CRC", "ERROR_ECC", "ERROR_STREAM_UNDERFLOW", "APPLY FAILED")):
@@ -365,6 +359,10 @@ class Controls:
         if not self.read_only:
           self.CI.init(self.CP, self.can_sock, self.pm.sock['sendcan'])
         self.initialized = True
+
+        if REPLAY and self.sm['pandaStates'][0].controlsAllowed:
+          self.state = State.enabled
+
         Params().put_bool("ControlsReady", True)
 
     # Check for CAN timeout
@@ -432,7 +430,7 @@ class Controls:
             # no more soft disabling condition, so go back to ENABLED
             self.state = State.enabled
 
-          elif self.events.any(ET.SOFT_DISABLE) and self.soft_disable_timer > 0:
+          elif self.soft_disable_timer > 0:
             self.current_alert_types.append(ET.SOFT_DISABLE)
 
           elif self.soft_disable_timer <= 0:
@@ -518,19 +516,8 @@ class Controls:
         lac_log.output = steer
         lac_log.saturated = abs(steer) >= 0.9
 
-    # Check for difference between desired angle and angle for angle based control
-    angle_control_saturated = self.CP.steerControlType == car.CarParams.SteerControlType.angle and \
-      abs(actuators.steeringAngleDeg - CS.steeringAngleDeg) > STEER_ANGLE_SATURATION_THRESHOLD
-
-    if angle_control_saturated and not CS.steeringPressed and self.active:
-      self.saturated_count += 1
-    else:
-      self.saturated_count = 0
-
     # Send a "steering required alert" if saturation count has reached the limit
-    if (lac_log.saturated and not CS.steeringPressed) or \
+    if lac_log.active and lac_log.saturated and not CS.steeringPressed:
-       (self.saturated_count > STEER_ANGLE_SATURATION_TIMEOUT):
-
       dpath_points = lat_plan.dPathPoints
       if len(dpath_points):
         # Check if we deviated from the path
@@ -593,7 +580,7 @@ class Controls:
     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
 
-    model_v2 = self.sm['modelV2'] 
+    model_v2 = self.sm['modelV2']
     desire_prediction = model_v2.meta.desirePrediction
     if len(desire_prediction) and ldw_allowed:
       right_lane_visible = self.sm['lateralPlan'].rProb > 0.5
@@ -611,10 +598,15 @@ class Controls:
     if hudControl.rightLaneDepart or hudControl.leftLaneDepart:
       self.events.add(EventName.ldw)
 
-    clear_event = ET.WARNING if ET.WARNING not in self.current_alert_types else None
+    clear_event_types = set()
+    if ET.WARNING not in self.current_alert_types:
+      clear_event_types.add(ET.WARNING)
+    if self.enabled:
+      clear_event_types.add(ET.NO_ENTRY)
+
     alerts = self.events.create_alerts(self.current_alert_types, [self.CP, self.sm, self.is_metric, self.soft_disable_timer])
     self.AM.add_many(self.sm.frame, alerts)
-    current_alert = self.AM.process_alerts(self.sm.frame, clear_event)
+    current_alert = self.AM.process_alerts(self.sm.frame, clear_event_types)
     if current_alert:
       hudControl.visualAlert = current_alert.visual_alert
 

selfdrive/controls/lib/alertmanager.py

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

selfdrive/controls/lib/alerts_offroad.json

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

selfdrive/controls/lib/desire_helper.py

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

selfdrive/controls/lib/events.py

@@ -264,8 +264,6 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = {
 
   EventName.stockFcw: {},
 
-  EventName.lkasDisabled: {},
-
   # ********** events only containing alerts displayed in all states **********
 
   EventName.joystickDebug: {
@@ -316,14 +314,6 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = {
     #ET.PERMANENT: ImmediateDisableAlert("openpilot failed to cancel cruise"),
   },
 
-  # Some features or cars are marked as community features. If openpilot
-  # detects the use of a community feature it switches to dashcam mode
-  # until these features are allowed using a toggle in settings.
-  EventName.communityFeatureDisallowed: {
-    ET.PERMANENT: NormalPermanentAlert("openpilot Unavailable",
-                                       "Enable Community Features in Settings"),
-  },
-
   # openpilot doesn't recognize the car. This switches openpilot into a
   # read-only mode. This can be solved by adding your fingerprint.
   # See https://github.com/commaai/openpilot/wiki/Fingerprinting for more information
@@ -516,7 +506,7 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = {
   # current GPS position. This alert is thrown when the localizer is reset
   # more often than expected.
   EventName.localizerMalfunction: {
-    ET.PERMANENT: NormalPermanentAlert("Sensor Malfunction", "Contact Support"),
+    # ET.PERMANENT: NormalPermanentAlert("Sensor Malfunction", "Contact Support"),
   },
 
   # ********** events that affect controls state transitions **********
@@ -831,4 +821,9 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = {
     ET.NO_ENTRY: NoEntryAlert("Cruise Fault: Restart the Car"),
   },
 
+  EventName.lkasDisabled: {
+    ET.PERMANENT: NormalPermanentAlert("LKAS Disabled: Enable LKAS to engage"),
+    ET.NO_ENTRY: NoEntryAlert("LKAS Disabled"),
+  },
+
 }

selfdrive/controls/lib/latcontrol.py

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

selfdrive/controls/lib/latcontrol_angle.py

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

selfdrive/controls/lib/latcontrol_indi.py

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

selfdrive/controls/lib/latcontrol_lqr.py

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

selfdrive/controls/lib/latcontrol_pid.py

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

selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py

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

selfdrive/controls/lib/lateral_planner.py

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

selfdrive/controls/lib/longcontrol.py

@@ -44,8 +44,7 @@ class LongControl():
     self.long_control_state = LongCtrlState.off  # initialized to off
     self.pid = PIController((CP.longitudinalTuning.kpBP, CP.longitudinalTuning.kpV),
                             (CP.longitudinalTuning.kiBP, CP.longitudinalTuning.kiV),
-                            rate=1/DT_CTRL,
+                            rate=1 / DT_CTRL)
-                            sat_limit=0.8)
     self.v_pid = 0.0
     self.last_output_accel = 0.0
 

selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py

@@ -215,6 +215,7 @@ class LongitudinalMpc:
     self.status = False
     self.crash_cnt = 0.0
     self.solution_status = 0
+    self.solve_time = 0.0
     self.x0 = np.zeros(X_DIM)
     self.set_weights()
 
@@ -356,7 +357,11 @@ class LongitudinalMpc:
       self.solver.set(i, 'p', self.params[i])
     self.solver.constraints_set(0, "lbx", self.x0)
     self.solver.constraints_set(0, "ubx", self.x0)
+
+    t = sec_since_boot()
     self.solution_status = self.solver.solve()
+    self.solve_time = sec_since_boot() - t
+
     for i in range(N+1):
       self.x_sol[i] = self.solver.get(i, 'x')
     for i in range(N):
@@ -368,7 +373,6 @@ class LongitudinalMpc:
 
     self.prev_a = np.interp(T_IDXS + 0.05, T_IDXS, self.a_solution)
 
-    t = sec_since_boot()
     if self.solution_status != 0:
       if t > self.last_cloudlog_t + 5.0:
         self.last_cloudlog_t = t

selfdrive/controls/lib/longitudinal_planner.py

@@ -120,4 +120,6 @@ class Planner:
     longitudinalPlan.longitudinalPlanSource = self.mpc.source
     longitudinalPlan.fcw = self.fcw
 
+    longitudinalPlan.solverExecutionTime = self.mpc.solve_time
+
     pm.send('longitudinalPlan', plan_send)

selfdrive/controls/lib/pid.py

@@ -13,7 +13,7 @@ def apply_deadzone(error, deadzone):
   return error
 
 class PIController():
-  def __init__(self, k_p, k_i, k_f=1., pos_limit=None, neg_limit=None, rate=100, sat_limit=0.8):
+  def __init__(self, k_p, k_i, k_f=1., pos_limit=None, neg_limit=None, rate=100):
     self._k_p = k_p  # proportional gain
     self._k_i = k_i  # integral gain
     self.k_f = k_f   # feedforward gain
@@ -25,10 +25,8 @@ class PIController():
     self.pos_limit = pos_limit
     self.neg_limit = neg_limit
 
-    self.sat_count_rate = 1.0 / rate
     self.i_unwind_rate = 0.3 / rate
     self.i_rate = 1.0 / rate
-    self.sat_limit = sat_limit
 
     self.reset()
 
@@ -40,27 +38,13 @@ class PIController():
   def k_i(self):
     return interp(self.speed, self._k_i[0], self._k_i[1])
 
-  def _check_saturation(self, control, check_saturation, error):
-    saturated = (control < self.neg_limit) or (control > self.pos_limit)
-
-    if saturated and check_saturation and abs(error) > 0.1:
-      self.sat_count += self.sat_count_rate
-    else:
-      self.sat_count -= self.sat_count_rate
-
-    self.sat_count = clip(self.sat_count, 0.0, 1.0)
-
-    return self.sat_count > self.sat_limit
-
   def reset(self):
     self.p = 0.0
     self.i = 0.0
     self.f = 0.0
-    self.sat_count = 0.0
-    self.saturated = False
     self.control = 0
 
-  def update(self, setpoint, measurement, speed=0.0, check_saturation=True, override=False, feedforward=0., deadzone=0., freeze_integrator=False):
+  def update(self, setpoint, measurement, speed=0.0, override=False, feedforward=0., deadzone=0., freeze_integrator=False):
     self.speed = speed
 
     error = float(apply_deadzone(setpoint - measurement, deadzone))
@@ -81,7 +65,6 @@ class PIController():
         self.i = i
 
     control = self.p + self.f + self.i
-    self.saturated = self._check_saturation(control, check_saturation, error)
 
     self.control = clip(control, self.neg_limit, self.pos_limit)
     return self.control

selfdrive/controls/lib/tests/test_alertmanager.py

@@ -34,7 +34,7 @@ class TestAlertManager(unittest.TestCase):
         for frame in range(duration+10):
           if frame < add_duration:
             AM.add_many(frame, [alert, ])
-          current_alert = AM.process_alerts(frame)
+          current_alert = AM.process_alerts(frame, {})
 
           shown = current_alert is not None
           should_show = frame <= show_duration

selfdrive/controls/lib/tests/test_latcontrol.py

@@ -0,0 +1,44 @@
+#!/usr/bin/env python3
+import unittest
+
+from parameterized import parameterized
+
+from cereal import car, log
+from selfdrive.car.car_helpers import interfaces
+from selfdrive.car.honda.values import CAR as HONDA
+from selfdrive.car.toyota.values import CAR as TOYOTA
+from selfdrive.car.nissan.values import CAR as NISSAN
+from selfdrive.controls.lib.latcontrol_pid import LatControlPID
+from selfdrive.controls.lib.latcontrol_lqr import LatControlLQR
+from selfdrive.controls.lib.latcontrol_indi import LatControlINDI
+from selfdrive.controls.lib.latcontrol_angle import LatControlAngle
+from selfdrive.controls.lib.vehicle_model import VehicleModel
+
+
+class TestLatControl(unittest.TestCase):
+
+  @parameterized.expand([(HONDA.CIVIC, LatControlPID), (TOYOTA.RAV4, LatControlLQR), (TOYOTA.PRIUS, LatControlINDI), (NISSAN.LEAF, LatControlAngle)])
+  def test_saturation(self, car_name, controller):
+    CarInterface, CarController, CarState = interfaces[car_name]
+    CP = CarInterface.get_params(car_name)
+    CI = CarInterface(CP, CarController, CarState)
+    VM = VehicleModel(CP)
+
+    controller = controller(CP, CI)
+
+
+    CS = car.CarState.new_message()
+    CS.vEgo = 30
+
+    last_actuators = car.CarControl.Actuators.new_message()
+
+    params = log.LiveParametersData.new_message()
+
+    for _ in range(1000):
+      _, _, lac_log = controller.update(True, CS, CP, VM, params, last_actuators, 1, 0)
+
+    self.assertTrue(lac_log.saturated)
+
+
+if __name__ == "__main__":
+  unittest.main()

selfdrive/controls/radard.py

@@ -172,7 +172,7 @@ class RadarD():
     radarState.carStateMonoTime = sm.logMonoTime['carState']
 
     if enable_lead:
-      leads_v3 = sm['modelV2'].leadsV3 
+      leads_v3 = sm['modelV2'].leadsV3
       if len(leads_v3) > 1:
         radarState.leadOne = get_lead(self.v_ego, self.ready, clusters, leads_v3[0], low_speed_override=True)
         radarState.leadTwo = get_lead(self.v_ego, self.ready, clusters, leads_v3[1], low_speed_override=False)