Merge remote-tracking branch 'upstream/master' into civic22_long

.github/ISSUE_TEMPLATE/bug_report.yml

@@ -8,6 +8,7 @@ body:
       value: >
         Before creating a **bug report**, please check the following:
           * If the issue likely only affects your car model or make, go back and open a **car bug report** instead.
+          * If the issue is related to the driving or driver monitoring models, you should open a [discussion](https://github.com/commaai/openpilot/discussions/categories/model-feedback) instead.
           * Ensure you're running the latest openpilot release.
           * Ensure you're using officially supported hardware. Issues running on PCs have a different issue template.
           * Ensure there isn't an existing issue for your bug. If there is, leave a comment on the existing issue.

.github/ISSUE_TEMPLATE/config.yml

@@ -1,8 +1,11 @@
 blank_issues_enabled: false
 contact_links:
+  - name: Report model bugs
+    url: https://github.com/commaai/openpilot/discussions/categories/model-feedback
+    about: Provide feedback for the driving or driver monitoring models
   - name: Discussions
     url: https://github.com/commaai/openpilot/discussions
-    about: For questions and discussion about openpilot
+    about: For questions and general discussion about openpilot
   - name: Community Wiki
     url: https://github.com/commaai/openpilot/wiki
     about: Check out our community wiki

.github/workflows/prebuilt.yaml

@@ -2,7 +2,6 @@ name: prebuilt
 on:
   schedule:
     - cron: '0 * * * *'
-
   workflow_dispatch:
 
 env:
@@ -11,9 +10,7 @@ env:
 
   DOCKER_LOGIN: docker login ghcr.io -u ${{ github.actor }} -p ${{ secrets.GITHUB_TOKEN }}
   BUILD: |
-      docker pull $(grep -iohP '(?<=^from)\s+\S+' Dockerfile.openpilot_base) || true
+      DOCKER_BUILDKIT=1 docker build --pull --build-arg BUILDKIT_INLINE_CACHE=1 --cache-from $DOCKER_REGISTRY/$BASE_IMAGE:latest -t $DOCKER_REGISTRY/$BASE_IMAGE:latest -t $BASE_IMAGE:latest -f Dockerfile.openpilot_base .
-      docker pull $DOCKER_REGISTRY/$BASE_IMAGE:latest || true
-      docker build --cache-from $DOCKER_REGISTRY/$BASE_IMAGE:latest -t $DOCKER_REGISTRY/$BASE_IMAGE:latest -t $BASE_IMAGE:latest -f Dockerfile.openpilot_base .
 
 jobs:
   build_prebuilt:
@@ -37,8 +34,7 @@ jobs:
     - name: Build Docker image
       run: |
         eval "$BUILD"
-        docker pull $DOCKER_REGISTRY/$IMAGE_NAME:latest || true
+        DOCKER_BUILDKIT=1 docker build --pull --build-arg BUILDKIT_INLINE_CACHE=1 --cache-from $DOCKER_REGISTRY/$IMAGE_NAME:latest -t $DOCKER_REGISTRY/$IMAGE_NAME:latest -f Dockerfile.openpilot .
-        docker build --cache-from $DOCKER_REGISTRY/$IMAGE_NAME:latest -t $DOCKER_REGISTRY/$IMAGE_NAME:latest -f Dockerfile.openpilot .
     - name: Push to container registry
       run: |
         $DOCKER_LOGIN

.github/workflows/selfdrive_tests.yaml

@@ -1,10 +1,15 @@
 name: selfdrive
+
 on:
   push:
     branches-ignore:
       - 'testing-closet*'
   pull_request:
 
+concurrency:
+  group: ${{ github.workflow }}-${{ github.ref != 'refs/heads/master' && github.ref || github.run_id }}-${{ github.event_name }}
+  cancel-in-progress: true
+
 env:
   BASE_IMAGE: openpilot-base
   CL_BASE_IMAGE: openpilot-base-cl
@@ -13,15 +18,12 @@ env:
 
   DOCKER_LOGIN: docker login ghcr.io -u ${{ github.actor }} -p ${{ secrets.GITHUB_TOKEN }}
   BUILD: |
-      docker pull $(grep -iohP '(?<=^from)\s+\S+' Dockerfile.openpilot_base) || true
+      DOCKER_BUILDKIT=1 docker build --pull --build-arg BUILDKIT_INLINE_CACHE=1 --cache-from $DOCKER_REGISTRY/$BASE_IMAGE:latest -t $DOCKER_REGISTRY/$BASE_IMAGE:latest -t $BASE_IMAGE:latest -f Dockerfile.openpilot_base .
-      docker pull $DOCKER_REGISTRY/$BASE_IMAGE:latest || true
-      docker build --cache-from $DOCKER_REGISTRY/$BASE_IMAGE:latest -t $DOCKER_REGISTRY/$BASE_IMAGE:latest -t $BASE_IMAGE:latest -f Dockerfile.openpilot_base .
 
   RUN: docker run --shm-size 1G -v $PWD:/tmp/openpilot -w /tmp/openpilot -e FILEREADER_CACHE=1 -e PYTHONPATH=/tmp/openpilot -e NUM_JOBS -e JOB_ID -e GITHUB_ACTION -e GITHUB_REF -e GITHUB_HEAD_REF -e GITHUB_SHA -e GITHUB_REPOSITORY -e GITHUB_RUN_ID -v /tmp/scons_cache:/tmp/scons_cache -v /tmp/comma_download_cache:/tmp/comma_download_cache -v /tmp/openpilot_cache:/tmp/openpilot_cache $BASE_IMAGE /bin/sh -c
 
   BUILD_CL: |
-      docker pull $DOCKER_REGISTRY/$CL_BASE_IMAGE:latest || true
+      DOCKER_BUILDKIT=1 docker build --pull --build-arg BUILDKIT_INLINE_CACHE=1 --cache-from $DOCKER_REGISTRY/$CL_BASE_IMAGE:latest -t $DOCKER_REGISTRY/$CL_BASE_IMAGE:latest -t $CL_BASE_IMAGE:latest -f Dockerfile.openpilot_base_cl .
-      docker build --cache-from $DOCKER_REGISTRY/$CL_BASE_IMAGE:latest -t $DOCKER_REGISTRY/$CL_BASE_IMAGE:latest -t $CL_BASE_IMAGE:latest -f Dockerfile.openpilot_base_cl .
   RUN_CL: docker run --shm-size 1G -v $PWD:/tmp/openpilot -w /tmp/openpilot -e PYTHONPATH=/tmp/openpilot -e NUM_JOBS -e JOB_ID -e GITHUB_ACTION -e GITHUB_REF -e GITHUB_HEAD_REF -e GITHUB_SHA -e GITHUB_REPOSITORY -e GITHUB_RUN_ID -v /tmp/scons_cache:/tmp/scons_cache -v /tmp/comma_download_cache:/tmp/comma_download_cache -v /tmp/openpilot_cache:/tmp/openpilot_cache $CL_BASE_IMAGE /bin/sh -c
 
   UNIT_TEST: coverage run --append -m unittest discover
@@ -36,13 +38,10 @@ jobs:
     steps:
     - uses: actions/checkout@v3
       with:
-        fetch-depth: 0
         submodules: true
     - name: Build devel
       run: TARGET_DIR=$STRIPPED_DIR release/build_devel.sh
     - uses: ./.github/workflows/setup
-      with:
-        save-cache: true
     - name: Check submodules
       if: github.ref == 'refs/heads/master' && github.repository == 'commaai/openpilot'
       run: release/check-submodules.sh
@@ -62,23 +61,25 @@ jobs:
         cp .pylintrc $STRIPPED_DIR
         cp mypy.ini $STRIPPED_DIR
         cd $STRIPPED_DIR
-        ${{ env.RUN }} "pre-commit run --all"
+        ${{ env.RUN }} "SKIP=test_translations pre-commit run --all"
 
   build_all:
     name: build all
     runs-on: ubuntu-20.04
-    timeout-minutes: 50
+    timeout-minutes: 30
     steps:
     - uses: actions/checkout@v3
       with:
         submodules: true
     - uses: ./.github/workflows/setup
+      with:
+        save-cache: true
     - name: Build openpilot with all flags
       run: ${{ env.RUN }} "scons -j$(nproc) --extras && release/check-dirty.sh"
     - name: Cleanup scons cache
       run: |
           ${{ env.RUN }} "rm -rf /tmp/scons_cache/* && \
-                          scons -j$(nproc) --extras --cache-populate"
+                          scons -j$(nproc) --cache-populate"
 
   #build_mac:
   #  name: build macos
@@ -140,7 +141,7 @@ jobs:
   docker_push:
     name: docker push
     runs-on: ubuntu-20.04
-    timeout-minutes: 50
+    timeout-minutes: 22
     if: github.ref == 'refs/heads/master' && github.event_name != 'pull_request' && github.repository == 'commaai/openpilot'
     needs: static_analysis # hack to ensure slow tests run first since this and static_analysis are fast
     steps:
@@ -149,12 +150,14 @@ jobs:
         submodules: true
     - name: Build Docker image
       run: eval "$BUILD"
+      timeout-minutes: 13
     - name: Push to container registry
       run: |
         $DOCKER_LOGIN
         docker push $DOCKER_REGISTRY/$BASE_IMAGE:latest
     - name: Build CL Docker image
       run: eval "$BUILD_CL"
+      timeout-minutes: 4
     - name: Push to container registry
       run: |
         $DOCKER_LOGIN
@@ -163,7 +166,7 @@ jobs:
   static_analysis:
     name: static analysis
     runs-on: ubuntu-20.04
-    timeout-minutes: 50
+    timeout-minutes: 20
     steps:
     - uses: actions/checkout@v3
       with:
@@ -171,21 +174,23 @@ jobs:
     - name: Build Docker image
       run: eval "$BUILD"
     - name: pre-commit
+      timeout-minutes: 5
       run: ${{ env.RUN }} "pre-commit run --all"
 
   valgrind:
     name: valgrind
     runs-on: ubuntu-20.04
-    timeout-minutes: 50
+    timeout-minutes: 20
     steps:
     - uses: actions/checkout@v3
       with:
         submodules: true
     - uses: ./.github/workflows/setup
+    - name: Build openpilot
+      run: ${{ env.RUN }} "scons -j$(nproc)"
     - name: Run valgrind
       run: |
-        ${{ env.RUN }} "scons -j$(nproc) && \
+        ${{ env.RUN }} "python selfdrive/test/test_valgrind_replay.py"
-                        python selfdrive/test/test_valgrind_replay.py"
     - name: Print logs
       if: always()
       run: cat selfdrive/test/valgrind_logs.txt
@@ -193,16 +198,18 @@ jobs:
   unit_tests:
     name: unit tests
     runs-on: ubuntu-20.04
-    timeout-minutes: 50
+    timeout-minutes: 30
     steps:
     - uses: actions/checkout@v3
       with:
         submodules: true
     - uses: ./.github/workflows/setup
+    - name: Build openpilot
+      run: ${{ env.RUN }} "scons -j$(nproc)"
     - name: Run unit tests
+      timeout-minutes: 15
       run: |
         ${{ env.RUN }} "export SKIP_LONG_TESTS=1 && \
-                        scons -j$(nproc) && \
                         $UNIT_TEST common && \
                         $UNIT_TEST opendbc/can && \
                         $UNIT_TEST selfdrive/boardd && \
@@ -215,7 +222,6 @@ jobs:
                         $UNIT_TEST selfdrive/athena && \
                         $UNIT_TEST selfdrive/thermald && \
                         $UNIT_TEST system/hardware/tici && \
-                        $UNIT_TEST selfdrive/modeld && \
                         $UNIT_TEST tools/lib/tests && \
                         ./selfdrive/ui/tests/create_test_translations.sh && \
                         QT_QPA_PLATFORM=offscreen ./selfdrive/ui/tests/test_translations && \
@@ -234,7 +240,7 @@ jobs:
   process_replay:
     name: process replay
     runs-on: ubuntu-20.04
-    timeout-minutes: 50
+    timeout-minutes: 25
     steps:
     - uses: actions/checkout@v3
       with:
@@ -246,10 +252,12 @@ jobs:
       with:
         path: /tmp/comma_download_cache
         key: proc-replay-${{ hashFiles('.github/workflows/selfdrive_tests.yaml', 'selfdrive/test/process_replay/ref_commit') }}
+    - name: Build openpilot
+      run: |
+        ${{ env.RUN }} "scons -j$(nproc)"
     - name: Run replay
       run: |
-        ${{ env.RUN }} "scons -j$(nproc) && \
+        ${{ env.RUN }} "CI=1 coverage run selfdrive/test/process_replay/test_processes.py -j$(nproc) && \
-                        CI=1 coverage run selfdrive/test/process_replay/test_processes.py -j$(nproc) && \
                         coverage xml"
     - name: Print diff
       if: always()
@@ -263,15 +271,14 @@ jobs:
     - name: Upload reference logs
       if: ${{ failure() && github.event_name == 'pull_request' && github.repository == 'commaai/openpilot' && env.AZURE_TOKEN != '' }}
       run: |
-        ${{ env.RUN }} "scons -j$(nproc) && \
+        ${{ env.RUN }} "CI=1 AZURE_TOKEN='$AZURE_TOKEN' python selfdrive/test/process_replay/test_processes.py -j$(nproc) --upload-only"
-                        CI=1 AZURE_TOKEN='$AZURE_TOKEN' python selfdrive/test/process_replay/test_processes.py -j$(nproc) --upload-only"
     - name: "Upload coverage to Codecov"
       uses: codecov/codecov-action@v2
 
-  model_replay_onnx:
+  test_modeld:
-    name: model replay onnx
+    name: model tests
     runs-on: ubuntu-20.04
-    timeout-minutes: 50
+    timeout-minutes: 20
     steps:
     - uses: actions/checkout@v3
       with:
@@ -280,29 +287,41 @@ jobs:
     - name: Build Docker image
       # Sim docker is needed to get the OpenCL drivers
       run: eval "$BUILD_CL"
-    - name: Run replay
+    - name: Build openpilot
+      run: |
+        ${{ env.RUN }} "scons -j$(nproc)"
+    - name: Run model replay with ONNX
+      timeout-minutes: 2
+      run: |
+        ${{ env.RUN_CL }} "ONNXCPU=1 CI=1 coverage run selfdrive/test/process_replay/model_replay.py && \
+                           coverage xml"
+    - name: Run unit tests
+      timeout-minutes: 5
       run: |
-        ${{ env.RUN_CL }} "scons -j$(nproc) && \
+        ${{ env.RUN_CL }} "$UNIT_TEST selfdrive/modeld && \
-                           ONNXCPU=1 CI=1 coverage run \
-                           selfdrive/test/process_replay/model_replay.py -j$(nproc) && \
                            coverage xml"
+    - name: "Upload coverage to Codecov"
+      uses: codecov/codecov-action@v2
 
   test_longitudinal:
     name: longitudinal
     runs-on: ubuntu-20.04
-    timeout-minutes: 50
+    timeout-minutes: 20
     steps:
     - uses: actions/checkout@v3
       with:
         submodules: true
     - uses: ./.github/workflows/setup
+    - name: Build openpilot
+      run: |
+        ${{ env.RUN }} "scons -j$(nproc)"
     - name: Test longitudinal
       run: |
         ${{ env.RUN }} "mkdir -p selfdrive/test/out && \
-                        scons -j$(nproc) && \
                         cd selfdrive/test/longitudinal_maneuvers && \
                         coverage run ./test_longitudinal.py && \
                         coverage xml"
+      timeout-minutes: 2
     - name: "Upload coverage to Codecov"
       uses: codecov/codecov-action@v2
     - uses: actions/upload-artifact@v2
@@ -315,7 +334,7 @@ jobs:
   test_cars:
     name: cars
     runs-on: ubuntu-20.04
-    timeout-minutes: 50
+    timeout-minutes: 20
     strategy:
       fail-fast: false
       matrix:
@@ -331,10 +350,12 @@ jobs:
       with:
         path: /tmp/comma_download_cache
         key: car_models-${{ hashFiles('selfdrive/car/tests/test_models.py', 'selfdrive/car/tests/routes.py') }}-${{ matrix.job }}
+    - name: Build openpilot
+      run: ${{ env.RUN }} "scons -j$(nproc)"
     - name: Test car models
+      timeout-minutes: 12
       run: |
-        ${{ env.RUN }} "scons -j$(nproc) && \
+        ${{ env.RUN }} "coverage run -m pytest selfdrive/car/tests/test_models.py && \
-                        coverage run -m pytest selfdrive/car/tests/test_models.py && \
                         coverage xml && \
                         chmod -R 777 /tmp/comma_download_cache"
       env:
@@ -343,29 +364,10 @@ jobs:
     - name: "Upload coverage to Codecov"
       uses: codecov/codecov-action@v2
 
-  docs:
-    name: build docs
-    runs-on: ubuntu-20.04
-    timeout-minutes: 50
-    steps:
-    - uses: actions/checkout@v3
-      with:
-        submodules: true
-    - name: Build docker container
-      run: |
-        docker pull $DOCKER_REGISTRY/$BASE_IMAGE:latest || true
-        docker pull $DOCKER_REGISTRY/openpilot-docs:latest || true
-        DOCKER_BUILDKIT=1 docker build --cache-from $DOCKER_REGISTRY/openpilot-docs:latest -t $DOCKER_REGISTRY/openpilot-docs:latest -f docs/docker/Dockerfile .
-    - name: Push docker container
-      if: github.ref == 'refs/heads/master' && github.event_name != 'pull_request' && github.repository == 'commaai/openpilot'
-      run: |
-        $DOCKER_LOGIN
-        docker push $DOCKER_REGISTRY/openpilot-docs:latest
-
   car_docs_diff:
-    name: comment on PR with car docs diff
+    name: PR comments
     runs-on: ubuntu-20.04
-    timeout-minutes: 50
+    timeout-minutes: 20
     if: github.event_name == 'pull_request'
     steps:
       - uses: actions/checkout@v3

.github/workflows/setup/action.yaml

@@ -15,9 +15,9 @@ runs:
     # build cache
     - id: date
       shell: bash
-      run: echo "::set-output name=date::$(git log -1 --pretty='format:%cd' --date=format:'%Y-%m-%d')"
+      run: echo "CACHE_COMMIT_DATE=$(git log -1 --pretty='format:%cd' --date=format:'%Y-%m-%d-%H:%M')" >> $GITHUB_ENV
     - shell: bash
-      run: echo "${{ steps.date.outputs.date }}"
+      run: echo "$CACHE_COMMIT_DATE"
     - shell: bash
       run: echo "CACHE_SKIP_SAVE=true" >> $GITHUB_ENV
       if: github.ref != 'refs/heads/master' || inputs.save-cache == 'false'
@@ -27,9 +27,9 @@ runs:
       uses: actions/cache@03e00da99d75a2204924908e1cca7902cafce66b
       with:
         path: /tmp/scons_cache
-        key: scons-${{ steps.date.outputs.date }}-${{ github.sha }}
+        key: scons-${{ env.CACHE_COMMIT_DATE }}-${{ github.sha }}
         restore-keys: |
-          scons-${{ steps.date.outputs.date }}-
+          scons-${{ env.CACHE_COMMIT_DATE }}-
           scons-
 
     # build our docker image

.github/workflows/tools_tests.yaml

@@ -1,8 +1,15 @@
 name: tools
+
 on:
   push:
+    branches-ignore:
+      - 'testing-closet*'
   pull_request:
 
+concurrency:
+  group: ${{ github.workflow }}-${{ github.ref != 'refs/heads/master' && github.ref || github.run_id }}-${{ github.event_name }}
+  cancel-in-progress: true
+
 env:
   BASE_IMAGE: openpilot-base
   CL_BASE_IMAGE: openpilot-base-cl
@@ -10,21 +17,20 @@ env:
   DOCKER_LOGIN: docker login ghcr.io -u ${{ github.actor }} -p ${{ secrets.GITHUB_TOKEN }}
 
   BUILD: |
-      docker pull $(grep -iohP '(?<=^from)\s+\S+' Dockerfile.openpilot_base) || true
+      DOCKER_BUILDKIT=1 docker build --pull --build-arg BUILDKIT_INLINE_CACHE=1 --cache-from $DOCKER_REGISTRY/$BASE_IMAGE:latest -t $DOCKER_REGISTRY/$BASE_IMAGE:latest -t $BASE_IMAGE:latest -f Dockerfile.openpilot_base .
-      docker pull $DOCKER_REGISTRY/$BASE_IMAGE:latest || true
+
-      docker build --cache-from $DOCKER_REGISTRY/$BASE_IMAGE:latest -t $DOCKER_REGISTRY/$BASE_IMAGE:latest -t $BASE_IMAGE:latest -f Dockerfile.openpilot_base .
+  RUN: docker run --shm-size 1G -v $GITHUB_WORKSPACE:/tmp/openpilot -w /tmp/openpilot -e FILEREADER_CACHE=1 -e PYTHONPATH=/tmp/openpilot -e NUM_JOBS -e JOB_ID -e GITHUB_ACTION -e GITHUB_REF -e GITHUB_HEAD_REF -e GITHUB_SHA -e GITHUB_REPOSITORY -e GITHUB_RUN_ID -v /tmp/scons_cache:/tmp/scons_cache -v /tmp/comma_download_cache:/tmp/comma_download_cache -v /tmp/openpilot_cache:/tmp/openpilot_cache $BASE_IMAGE /bin/sh -c
+
   BUILD_CL: |
-      docker pull $(grep -iohP '(?<=^from)\s+\S+' Dockerfile.openpilot_base_cl) || true
+      DOCKER_BUILDKIT=1 docker build --pull --build-arg BUILDKIT_INLINE_CACHE=1 --cache-from $DOCKER_REGISTRY/$CL_BASE_IMAGE:latest -t $DOCKER_REGISTRY/$CL_BASE_IMAGE:latest -t $CL_BASE_IMAGE:latest -f Dockerfile.openpilot_base_cl .
-      docker pull $DOCKER_REGISTRY/$BASE_IMAGE:latest || true
+  RUN_CL: docker run --shm-size 1G -v $GITHUB_WORKSPACE:/tmp/openpilot -w /tmp/openpilot -e PYTHONPATH=/tmp/openpilot -e NUM_JOBS -e JOB_ID -e GITHUB_ACTION -e GITHUB_REF -e GITHUB_HEAD_REF -e GITHUB_SHA -e GITHUB_REPOSITORY -e GITHUB_RUN_ID -v /tmp/scons_cache:/tmp/scons_cache -v /tmp/comma_download_cache:/tmp/comma_download_cache -v /tmp/openpilot_cache:/tmp/openpilot_cache $CL_BASE_IMAGE /bin/sh -c
-      docker build --cache-from $DOCKER_REGISTRY/$CL_BASE_IMAGE:latest -t $DOCKER_REGISTRY/$CL_BASE_IMAGE:latest -t $CL_BASE_IMAGE:latest -f Dockerfile.openpilot_base_cl .
+
-  RUN: docker run --shm-size 1G -v $PWD:/tmp/openpilot -e PYTHONPATH=/tmp/openpilot -e GITHUB_ACTION -e GITHUB_REF -e GITHUB_HEAD_REF -e GITHUB_SHA -e \
-       GITHUB_REPOSITORY -e GITHUB_RUN_ID -v /tmp/comma_download_cache:/tmp/comma_download_cache $BASE_IMAGE /bin/sh -c
 
 jobs:
   plotjuggler:
     name: plotjuggler
     runs-on: ubuntu-20.04
-    timeout-minutes: 30
+    timeout-minutes: 20
     steps:
     - uses: actions/checkout@v3
       with:
@@ -32,6 +38,7 @@ jobs:
     - name: Build Docker image
       run: eval "$BUILD"
     - name: Unit test
+      timeout-minutes: 2
       run: |
         ${{ env.RUN }} "scons -j$(nproc) --directory=/tmp/openpilot/cereal && \
                         apt-get update && \
@@ -42,7 +49,7 @@ jobs:
   simulator:
     name: simulator
     runs-on: ubuntu-20.04
-    timeout-minutes: 50
+    timeout-minutes: 30
     env:
       IMAGE_NAME: openpilot-sim
     if: github.repository == 'commaai/openpilot'
@@ -56,12 +63,29 @@ jobs:
       run: eval "$BUILD"
     - name: Build base cl image
       run: eval "$BUILD_CL"
-    - name: Pull latest simulator image
-      run: docker pull $DOCKER_REGISTRY/$IMAGE_NAME:latest || true
     - name: Build simulator image
-      run: docker build --cache-from $DOCKER_REGISTRY/$IMAGE_NAME:latest -t $DOCKER_REGISTRY/$IMAGE_NAME:latest -f tools/sim/Dockerfile.sim .
+      run: DOCKER_BUILDKIT=1 docker build --pull --build-arg BUILDKIT_INLINE_CACHE=1 --cache-from $DOCKER_REGISTRY/$IMAGE_NAME:latest -t $DOCKER_REGISTRY/$IMAGE_NAME:latest -f tools/sim/Dockerfile.sim .
     - name: Push to container registry
       if: github.ref == 'refs/heads/master' && github.repository == 'commaai/openpilot'
       run: |
         $DOCKER_LOGIN
         docker push $DOCKER_REGISTRY/$IMAGE_NAME:latest
+
+  docs:
+    name: build docs
+    runs-on: ubuntu-20.04
+    timeout-minutes: 25
+    steps:
+    - uses: actions/checkout@v3
+      with:
+        submodules: true
+    - name: Build docker container
+      run: |
+        DOCKER_BUILDKIT=1 docker build --pull --build-arg BUILDKIT_INLINE_CACHE=1 --cache-from $DOCKER_REGISTRY/openpilot-docs:latest -t $DOCKER_REGISTRY/openpilot-docs:latest -f docs/docker/Dockerfile .
+    - name: Push docker container
+      if: github.ref == 'refs/heads/master' && github.event_name != 'pull_request' && github.repository == 'commaai/openpilot'
+      run: |
+        $DOCKER_LOGIN
+        docker push $DOCKER_REGISTRY/openpilot-docs:latest
+
+

.gitmodules

@@ -18,4 +18,4 @@
   url = ../../commaai/body.git
 [submodule "tinygrad"]
   path = tinygrad_repo
-  url = https://github.com/geohot/tinygrad.git
+  url = ../../commaai/tinygrad.git

.pre-commit-config.yaml

@@ -24,18 +24,14 @@ repos:
         # if you've got a short variable name that's getting flagged, add it here
         - -L bu,ro,te,ue,alo,hda,ois,nam,nams,ned,som,parm,setts,inout,warmup
         - --builtins clear,rare,informal,usage,code,names,en-GB_to_en-US
--   repo: https://github.com/pre-commit/mirrors-mypy
+-   repo: local
-    rev: v0.931
     hooks:
     -   id: mypy
-        exclude: '^(pyextra/)|(cereal/)|(rednose/)|(panda/)|(laika/)|(opendbc/)|(laika_repo/)|(rednose_repo/)/|(tinygrad/)|(tinygrad_repo/)'
+        name: mypy
-        additional_dependencies: ['types-PyYAML', 'lxml', 'numpy', 'types-atomicwrites', 'types-pycurl', 'types-requests', 'types-certifi']
+        entry: mypy
-        args:
+        language: system
-        - --warn-redundant-casts
+        types: [python]
-        - --warn-return-any
+        exclude: '^(pyextra/)|(cereal/)|(opendbc/)|(panda/)|(laika/)|(laika_repo/)|(rednose/)|(rednose_repo/)|(tinygrad/)|(tinygrad_repo/)|(xx/)'
-        - --warn-unreachable
-        - --warn-unused-ignores
-        #- --html-report=/home/batman/openpilot
 -   repo: https://github.com/PyCQA/flake8
     rev: 4.0.1
     hooks:
@@ -57,6 +53,7 @@ repos:
         types: [python]
         exclude: '^(pyextra/)|(cereal/)|(rednose/)|(panda/)|(laika/)|(laika_repo/)|(rednose_repo/)|(tinygrad/)|(tinygrad_repo/)'
         args:
+        - -j0
         - -rn
         - -sn
         - --rcfile=.pylintrc
@@ -74,3 +71,10 @@ repos:
         - --quiet
         - --force
         - -j8
+-   repo: local
+    hooks:
+    -   id: test_translations
+        name: test translations
+        entry: selfdrive/ui/tests/test_translations.py
+        language: script
+        pass_filenames: false

Jenkinsfile

@@ -127,7 +127,7 @@ pipeline {
           steps {
             phone_steps("tici2", [
               ["build", "cd selfdrive/manager && ./build.py"],
-              //["test power draw", "python system/hardware/tici/test_power_draw.py"],
+              ["test power draw", "python system/hardware/tici/test_power_draw.py"],
               ["test boardd loopback", "python selfdrive/boardd/tests/test_boardd_loopback.py"],
               ["test loggerd", "python selfdrive/loggerd/tests/test_loggerd.py"],
               ["test encoder", "LD_LIBRARY_PATH=/usr/local/lib python selfdrive/loggerd/tests/test_encoder.py"],

Pipfile

@@ -43,6 +43,12 @@ carla = {version = "==0.9.13", markers="platform_system != 'Darwin'"}
 ft4222 = "*"
 pandas = "*"
 tabulate = "*"
+types-pyyaml = "*"
+lxml = "*"
+types-atomicwrites = "*"
+types-pycurl = "*"
+types-requests = "*"
+types-certifi = "*"
 
 [packages]
 atomicwrites = "*"

RELEASES.md

@@ -3,13 +3,14 @@ Version 0.8.17 (2022-XX-XX)
 * New driving model
   * Internal feature space accuracy increased tenfold during training, this makes the model dramatically more accurate.
 * Self-tuning torque lateral controller parameters
-  * Parameters are learned live for each car
+  * Parameters learned live for each car
   * Enabled only on Toyota Corolla for now
 * UI updates
+  * Multi-language in navigation
   * Matched speeds shown on car's dash
   * Improved update experience
   * Border turns grey while overriding steering
-  * Added button to flag events that are shown in comma connect
+  * Added button to bookmark events while driving; view them later in comma connect
 * AGNOS 6
 
 Version 0.8.16 (2022-08-26)

SConstruct

@@ -431,8 +431,13 @@ SConscript(['selfdrive/sensord/SConscript'])
 SConscript(['selfdrive/ui/SConscript'])
 SConscript(['selfdrive/navd/SConscript'])
 
+if arch in ['x86_64', 'Darwin'] or GetOption('extras'):
   SConscript(['tools/replay/SConscript'])
 
+  opendbc = abspath([File('opendbc/can/libdbc.so')])
+  Export('opendbc')
+  SConscript(['tools/cabana/SConscript'])
+
 if GetOption('test'):
   SConscript('panda/tests/safety/SConscript')
 

cereal

@@ -1 +1 @@
-Subproject commit 3baa20e1da5d88dcb1d3ae9678471eb8013958f2
+Subproject commit 5766e645f2ee2a131b145fb1ea9e3b7c55a4a740

common/params.cc

@@ -118,6 +118,7 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"GithubUsername", PERSISTENT},
     {"GitRemote", PERSISTENT},
     {"GsmApn", PERSISTENT},
+    {"GsmMetered", PERSISTENT},
     {"GsmRoaming", PERSISTENT},
     {"HardwareSerial", PERSISTENT},
     {"HasAcceptedTerms", PERSISTENT},
@@ -165,6 +166,7 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"TermsVersion", PERSISTENT},
     {"Timezone", PERSISTENT},
     {"TrainingVersion", PERSISTENT},
+    {"UbloxAvailable", PERSISTENT},
     {"UpdateAvailable", CLEAR_ON_MANAGER_START},
     {"UpdateFailedCount", CLEAR_ON_MANAGER_START},
     {"UpdaterState", CLEAR_ON_MANAGER_START},
@@ -297,12 +299,15 @@ std::map<std::string, std::string> Params::readAll() {
 void Params::clearAll(ParamKeyType key_type) {
   FileLock file_lock(params_path + "/.lock");
 
-  std::string path;
+  if (key_type == ALL) {
+    util::remove_files_in_dir(getParamPath());
+  } else {
     for (auto &[key, type] : keys) {
       if (type & key_type) {
         unlink(getParamPath(key).c_str());
       }
     }
+  }
 
   fsync_dir(getParamPath());
 }

common/params.h

@@ -29,8 +29,8 @@ public:
 
   // helpers for reading values
   std::string get(const std::string &key, bool block = false);
-  inline bool getBool(const std::string &key) {
+  inline bool getBool(const std::string &key, bool block = false) {
-    return get(key) == "1";
+    return get(key, block) == "1";
   }
   std::map<std::string, std::string> readAll();
 

common/params_pyx.pyx

@@ -16,7 +16,7 @@ cdef extern from "common/params.h":
   cdef cppclass c_Params "Params":
     c_Params(string) nogil
     string get(string, bool) nogil
-    bool getBool(string) nogil
+    bool getBool(string, bool) nogil
     int remove(string) nogil
     int put(string, string) nogil
     int putBool(string, bool) nogil
@@ -68,11 +68,11 @@ cdef class Params:
 
     return val if encoding is None else val.decode(encoding)
 
-  def get_bool(self, key):
+  def get_bool(self, key, bool block=False):
     cdef string k = self.check_key(key)
     cdef bool r
     with nogil:
-      r = self.p.getBool(k)
+      r = self.p.getBool(k, block)
     return r
 
   def put(self, key, dat):

common/realtime.py

@@ -31,7 +31,7 @@ class Priority:
 
 def set_realtime_priority(level: int) -> None:
   if not PC:
-    os.sched_setscheduler(0, os.SCHED_FIFO, os.sched_param(level))  # type: ignore[attr-defined] # pylint: disable=no-member
+    os.sched_setscheduler(0, os.SCHED_FIFO, os.sched_param(level))  # pylint: disable=no-member
 
 
 def set_core_affinity(cores: List[int]) -> None:

common/tests/test_util.cc

@@ -143,3 +143,20 @@ TEST_CASE("util::create_directories") {
     REQUIRE(util::create_directories("", 0755) == false);
   }
 }
+
+TEST_CASE("util::remove_files_in_dir") {
+  std::string tmp_dir = "/tmp/test_remove_all_in_dir";
+  system("rm /tmp/test_remove_all_in_dir -rf");
+  REQUIRE(util::create_directories(tmp_dir, 0755));
+  const int tmp_file_cnt = 10;
+  for (int i = 0; i < tmp_file_cnt; ++i) {
+    std::string tmp_file = tmp_dir + "/test_XXXXXX";
+    int fd = mkstemp((char*)tmp_file.c_str());
+    close(fd);
+    REQUIRE(util::file_exists(tmp_file.c_str()));
+  }
+
+  REQUIRE(util::read_files_in_dir(tmp_dir).size() == tmp_file_cnt);
+  util::remove_files_in_dir(tmp_dir);
+  REQUIRE(util::read_files_in_dir(tmp_dir).empty());
+}

common/util.cc

@@ -97,6 +97,22 @@ std::map<std::string, std::string> read_files_in_dir(const std::string &path) {
   return ret;
 }
 
+void remove_files_in_dir(const std::string &path) {
+  DIR *d = opendir(path.c_str());
+  if (!d) return;
+
+  std::string fn;
+  struct dirent *de = NULL;
+  while ((de = readdir(d))) {
+    if (de->d_type != DT_DIR) {
+      fn = path + "/" + de->d_name;
+      unlink(fn.c_str());
+    }
+  }
+
+  closedir(d);
+}
+
 int write_file(const char* path, const void* data, size_t size, int flags, mode_t mode) {
   int fd = HANDLE_EINTR(open(path, flags, mode));
   if (fd == -1) {

common/util.h

@@ -80,6 +80,7 @@ std::string dir_name(std::string const& path);
 // **** file fhelpers *****
 std::string read_file(const std::string& fn);
 std::map<std::string, std::string> read_files_in_dir(const std::string& path);
+void remove_files_in_dir(const std::string& path);
 int write_file(const char* path, const void* data, size_t size, int flags = O_WRONLY, mode_t mode = 0664);
 
 FILE* safe_fopen(const char* filename, const char* mode);

docs/CARS.md

@@ -4,7 +4,7 @@
 
 A supported vehicle is one that just works when you install a comma three. All supported cars provide a better experience than any stock system.
 
-# 204 Supported Cars
+# 208 Supported Cars
 
 |Make|Model|Supported Package|ACC|No ACC accel below|No ALC below|Steering Torque|Resume from stop|Harness|
 |---|---|---|:---:|:---:|:---:|:---:|:---:|:---:|
@@ -55,9 +55,10 @@ A supported vehicle is one that just works when you install a comma three. All s
 |Honda|Ridgeline 2017-22|Honda Sensing|openpilot|25 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Nidec|
 |Hyundai|Elantra 2017-19|Smart Cruise Control (SCC)|Stock|19 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai B|
 |Hyundai|Elantra 2021-22|Smart Cruise Control (SCC)|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai K|
-|Hyundai|Elantra Hybrid 2021-22|Smart Cruise Control (SCC)|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai K|
+|Hyundai|Elantra Hybrid 2021-23|Smart Cruise Control (SCC)|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai K|
 |Hyundai|Genesis 2015-16|Smart Cruise Control (SCC)|Stock|19 mph|37 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai J|
-|Hyundai|Ioniq 5 2022|Highway Driving Assist II|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai Q|
+|Hyundai|Ioniq 5 (with HDA II) 2022|Highway Driving Assist II|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai Q|
+|Hyundai|Ioniq 5 (without HDA II) 2022|Highway Driving Assist|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai K|
 |Hyundai|Ioniq Electric 2019|Smart Cruise Control (SCC)|Stock|0 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai C|
 |Hyundai|Ioniq Electric 2020|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai H|
 |Hyundai|Ioniq Hybrid 2017-19|Smart Cruise Control (SCC)|Stock|0 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai C|
@@ -83,7 +84,8 @@ A supported vehicle is one that just works when you install a comma three. All s
 |Jeep|Grand Cherokee 2016-18|Adaptive Cruise Control (ACC)|Stock|0 mph|9 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|FCA|
 |Jeep|Grand Cherokee 2019-21|Adaptive Cruise Control (ACC)|Stock|0 mph|39 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|FCA|
 |Kia|Ceed 2019|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai E|
-|Kia|EV6 2022|Highway Driving Assist II|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai P|
+|Kia|EV6 (with HDA II) 2022|Highway Driving Assist II|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai P|
+|Kia|EV6 (without HDA II) 2022|Highway Driving Assist|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai L|
 |Kia|Forte 2019-21|Smart Cruise Control (SCC)|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai G|
 |Kia|K5 2021-22|Smart Cruise Control (SCC)|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai A|
 |Kia|Niro EV 2019|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai H|
@@ -110,9 +112,11 @@ A supported vehicle is one that just works when you install a comma three. All s
 |Lexus|NX Hybrid 2018-19|All|Stock[<sup>3</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota|
 |Lexus|NX Hybrid 2020-21|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota|
 |Lexus|RC 2017-20|All|Stock|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota|
-|Lexus|RX 2016-19|All|Stock[<sup>3</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota|
+|Lexus|RX 2016|Lexus Safety System+|Stock[<sup>3</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota|
+|Lexus|RX 2017-19|All|Stock[<sup>3</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota|
 |Lexus|RX 2020-22|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota|
-|Lexus|RX Hybrid 2016-19|All|Stock[<sup>3</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota|
+|Lexus|RX Hybrid 2016|Lexus Safety System+|Stock[<sup>3</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota|
+|Lexus|RX Hybrid 2017-19|All|Stock[<sup>3</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota|
 |Lexus|RX Hybrid 2020-21|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota|
 |Lexus|UX Hybrid 2019-22|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota|
 |Mazda|CX-5 2022|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Mazda|

laika_repo

@@ -1 +1 @@
-Subproject commit c8bc1fa01be9f22592efb991ee52d3d965d21968
+Subproject commit e1049cde0a68f7d4a70b1ebd76befdc0e163ad55

launch_env.sh

@@ -7,7 +7,7 @@ export OPENBLAS_NUM_THREADS=1
 export VECLIB_MAXIMUM_THREADS=1
 
 if [ -z "$AGNOS_VERSION" ]; then
-  export AGNOS_VERSION="6"
+  export AGNOS_VERSION="6.1"
 fi
 
 if [ -z "$PASSIVE" ]; then

mypy.ini

@@ -1,4 +1,16 @@
 [mypy]
 python_version = 3.8
-ignore_missing_imports = True
 plugins = numpy.typing.mypy_plugin
+files = body, common, docs, scripts, selfdrive, site_scons, system, tools
+exclude = ^(pyextra/)|(cereal/)|(opendbc/)|(panda/)|(laika/)|(laika_repo/)|(rednose/)|(rednose_repo/)|(tinygrad/)|(tinygrad_repo/)|(xx/)
+
+; third-party packages
+ignore_missing_imports = True
+
+; helpful warnings
+warn_redundant_casts = True
+warn_unreachable = True
+warn_unused_ignores = True
+
+; restrict dynamic typing
+warn_return_any = True

opendbc

@@ -1 +1 @@
-Subproject commit eaac172af9cb342204e69ec52339cdf3c6a8ac4e
+Subproject commit dde0ff6f4456c167df204bf5ac1e3f2979c844c9

release/build_devel.sh

@@ -12,10 +12,7 @@ fi
 # set git identity
 source $DIR/identity.sh
 
-echo "[-] Setting up repo T=$SECONDS"
+echo "[-] Setting up target repo T=$SECONDS"
-
-cd $SOURCE_DIR
-git fetch origin
 
 rm -rf $TARGET_DIR
 mkdir -p $TARGET_DIR
@@ -25,14 +22,14 @@ pre-commit uninstall || true
 
 echo "[-] bringing master-ci and devel in sync T=$SECONDS"
 cd $TARGET_DIR
-git fetch origin master-ci
+git fetch --depth 1 origin master-ci
-git fetch origin devel
+git fetch --depth 1 origin devel
 
 git checkout -f --track origin/master-ci
 git reset --hard master-ci
 git checkout master-ci
 git reset --hard origin/devel
-git clean -xdf
+git clean -xdff
 git lfs uninstall
 
 # remove everything except .git
@@ -41,7 +38,7 @@ find . -maxdepth 1 -not -path './.git' -not -name '.' -not -name '..' -exec rm -
 
 # reset source tree
 cd $SOURCE_DIR
-git clean -xdf
+git clean -xdff
 
 # do the files copy
 echo "[-] copying files T=$SECONDS"

release/files_common

@@ -305,6 +305,8 @@ selfdrive/ui/soundd/soundd
 selfdrive/ui/soundd/.gitignore
 selfdrive/ui/translations/*.ts
 selfdrive/ui/translations/languages.json
+selfdrive/ui/update_translations.py
+selfdrive/ui/tests/test_translations.py
 
 selfdrive/ui/qt/*.cc
 selfdrive/ui/qt/*.h
@@ -572,3 +574,4 @@ tinygrad_repo/tinygrad/ops.py
 tinygrad_repo/tinygrad/shapetracker.py
 tinygrad_repo/tinygrad/tensor.py
 tinygrad_repo/tinygrad/nn/__init__.py
+tinygrad_repo/tinygrad/nn/optim.py

scripts/launch_corolla.sh

@@ -3,4 +3,5 @@
 DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" >/dev/null && pwd)"
 
 export FINGERPRINT="TOYOTA COROLLA TSS2 2019"
+export SKIP_FW_QUERY="1"
 $DIR/../launch_openpilot.sh

scripts/switch_to_master.sh

@@ -1,16 +0,0 @@
-#!/usr/bin/bash
-
-DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null && pwd )"
-cd $DIR/..
-
-git clean -xdf .
-git rm -r --cached .
-
-git config remote.origin.fetch "+refs/heads/*:refs/remotes/origin/*"
-git fetch origin master
-git checkout master
-git reset --hard
-git submodule update --init
-
-printf '\n\n'
-echo "master checked out. reboot to start building openpilot master"

selfdrive/athena/tests/helpers.py

@@ -53,8 +53,8 @@ class MockParams():
   default_params = {
     "DongleId": b"0000000000000000",
     "GithubSshKeys": b"ssh-rsa AAAAB3NzaC1yc2EAAAADAQABAAABAQC307aE+nuHzTAgaJhzSf5v7ZZQW9gaperjhCmyPyl4PzY7T1mDGenTlVTN7yoVFZ9UfO9oMQqo0n1OwDIiqbIFxqnhrHU0cYfj88rI85m5BEKlNu5RdaVTj1tcbaPpQc5kZEolaI1nDDjzV0lwS7jo5VYDHseiJHlik3HH1SgtdtsuamGR2T80q1SyW+5rHoMOJG73IH2553NnWuikKiuikGHUYBd00K1ilVAK2xSiMWJp55tQfZ0ecr9QjEsJ+J/efL4HqGNXhffxvypCXvbUYAFSddOwXUPo5BTKevpxMtH+2YrkpSjocWA04VnTYFiPG6U4ItKmbLOTFZtPzoez private",  # noqa: E501
+    "GsmMetered": True,
     "AthenadUploadQueue": '[]',
-    "CellularUnmetered": False,
   }
   params = default_params.copy()
 

selfdrive/boardd/boardd.cc

@@ -56,7 +56,6 @@
 using namespace std::chrono_literals;
 
 std::atomic<bool> ignition(false);
-std::atomic<bool> pigeon_active(false);
 
 ExitHandler do_exit;
 
@@ -114,8 +113,9 @@ bool safety_setter_thread(std::vector<Panda *> pandas) {
   }
 
   // set to ELM327 for fingerprinting
-  for (Panda *panda : pandas) {
+  for (int i = 0; i < pandas.size(); i++) {
-    panda->set_safety_model(cereal::CarParams::SafetyModel::ELM327);
+    const uint16_t safety_param = (i > 0) ? 1U : 0U;
+    pandas[i]->set_safety_model(cereal::CarParams::SafetyModel::ELM327, safety_param);
   }
 
   Params p = Params();
@@ -346,7 +346,7 @@ std::optional<bool> send_panda_states(PubMaster *pm, const std::vector<Panda *>
     }
 
   #ifndef __x86_64__
-    bool power_save_desired = !ignition_local && !pigeon_active;
+    bool power_save_desired = !ignition_local;
     if (health.power_save_enabled_pkt != power_save_desired) {
       panda->set_power_saving(power_save_desired);
     }
@@ -382,6 +382,7 @@ std::optional<bool> send_panda_states(PubMaster *pm, const std::vector<Panda *>
     ps.setHarnessStatus(cereal::PandaState::HarnessStatus(health.car_harness_status_pkt));
     ps.setInterruptLoad(health.interrupt_load);
     ps.setFanPower(health.fan_power);
+    ps.setSafetyRxChecksInvalid((bool)(health.safety_rx_checks_invalid));
 
     std::array<cereal::PandaState::PandaCanState::Builder, PANDA_CAN_CNT> cs = {ps.initCanState0(), ps.initCanState1(), ps.initCanState2()};
 
@@ -407,6 +408,7 @@ std::optional<bool> send_panda_states(PubMaster *pm, const std::vector<Panda *>
       cs[j].setCanDataSpeed(can_health.can_data_speed);
       cs[j].setCanfdEnabled(can_health.canfd_enabled);
       cs[j].setBrsEnabled(can_health.brs_enabled);
+      cs[j].setCanfdNonIso(can_health.canfd_non_iso);
     }
 
     // Convert faults bitset to capnp list

selfdrive/boardd/panda.cc

@@ -360,6 +360,10 @@ void Panda::set_data_speed_kbps(uint16_t bus, uint16_t speed) {
   usb_write(0xf9, bus, (speed * 10));
 }
 
+void Panda::set_canfd_non_iso(uint16_t bus, bool non_iso) {
+  usb_write(0xfc, bus, non_iso);
+}
+
 static uint8_t len_to_dlc(uint8_t len) {
   if (len <= 8) {
     return len;

selfdrive/boardd/panda.h

@@ -91,6 +91,7 @@ class Panda {
   void send_heartbeat(bool engaged);
   void set_can_speed_kbps(uint16_t bus, uint16_t speed);
   void set_data_speed_kbps(uint16_t bus, uint16_t speed);
+  void set_canfd_non_iso(uint16_t bus, bool non_iso);
   void can_send(capnp::List<cereal::CanData>::Reader can_data_list);
   bool can_receive(std::vector<can_frame>& out_vec);
 

selfdrive/car/__init__.py

@@ -3,7 +3,7 @@ import capnp
 
 from cereal import car
 from common.numpy_fast import clip
-from typing import Dict, List
+from typing import Dict
 
 # kg of standard extra cargo to count for drive, gas, etc...
 STD_CARGO_KG = 136.
@@ -32,19 +32,6 @@ def create_button_event(cur_but: int, prev_but: int, buttons_dict: Dict[int, cap
   return be
 
 
-def create_button_enable_events(buttonEvents: capnp.lib.capnp._DynamicListBuilder, pcm_cruise: bool = False) -> List[int]:
-  events = []
-  for b in buttonEvents:
-    # do enable on both accel and decel buttons
-    if not pcm_cruise:
-      if b.type in (ButtonType.accelCruise, ButtonType.decelCruise) and not b.pressed:
-        events.append(EventName.buttonEnable)
-    # do disable on button down
-    if b.type == ButtonType.cancel and b.pressed:
-      events.append(EventName.buttonCancel)
-  return events
-
-
 def gen_empty_fingerprint():
   return {i: {} for i in range(0, 8)}
 

selfdrive/car/car_helpers.py

@@ -81,7 +81,7 @@ def fingerprint(logcan, sendcan):
   skip_fw_query = os.environ.get('SKIP_FW_QUERY', False)
   ecu_rx_addrs = set()
 
-  if not fixed_fingerprint and not skip_fw_query:
+  if not skip_fw_query:
     # Vin query only reliably works through OBDII
     bus = 1
 
@@ -95,16 +95,19 @@ def fingerprint(logcan, sendcan):
       cloudlog.warning("Using cached CarParams")
       vin, vin_rx_addr = cached_params.carVin, 0
       car_fw = list(cached_params.carFw)
+      cached = True
     else:
       cloudlog.warning("Getting VIN & FW versions")
-      _, vin_rx_addr, vin = get_vin(logcan, sendcan, bus)
+      vin_rx_addr, vin = get_vin(logcan, sendcan, bus)
       ecu_rx_addrs = get_present_ecus(logcan, sendcan)
       car_fw = get_fw_versions_ordered(logcan, sendcan, ecu_rx_addrs)
+      cached = False
 
     exact_fw_match, fw_candidates = match_fw_to_car(car_fw)
   else:
     vin, vin_rx_addr = VIN_UNKNOWN, 0
     exact_fw_match, fw_candidates, car_fw = True, set(), []
+    cached = False
 
   if not is_valid_vin(vin):
     cloudlog.event("Malformed VIN", vin=vin, error=True)
@@ -165,7 +168,7 @@ def fingerprint(logcan, sendcan):
     car_fingerprint = fixed_fingerprint
     source = car.CarParams.FingerprintSource.fixed
 
-  cloudlog.event("fingerprinted", car_fingerprint=car_fingerprint, source=source, fuzzy=not exact_match,
+  cloudlog.event("fingerprinted", car_fingerprint=car_fingerprint, source=source, fuzzy=not exact_match, cached=cached,
                  fw_count=len(car_fw), ecu_responses=list(ecu_rx_addrs), vin_rx_addr=vin_rx_addr, error=True)
   return car_fingerprint, finger, vin, car_fw, source, exact_match
 

selfdrive/car/chrysler/values.py

@@ -166,14 +166,13 @@ FW_QUERY_CONFIG = FwQueryConfig(
       bus=0,
     ),
   ],
+  extra_ecus=[
+    (Ecu.hcp, 0x7e2, None),  # manages transmission on hybrids
+    (Ecu.abs, 0x7e4, None),  # alt address for abs on hybrids
+  ],
 )
 
 FW_VERSIONS = {
-  CAR.PACIFICA_2019_HYBRID: {
-    (Ecu.hcp, 0x7e2, None): [],
-    (Ecu.abs, 0x7e4, None): [],
-  },
-
   CAR.RAM_1500: {
     (Ecu.combinationMeter, 0x742, None): [
       b'68294063AH',

selfdrive/car/ecu_addrs.py

@@ -1,7 +1,6 @@
 #!/usr/bin/env python3
 import capnp
 import time
-import traceback
 from typing import Optional, Set, Tuple
 
 import cereal.messaging as messaging
@@ -62,7 +61,7 @@ def get_ecu_addrs(logcan: messaging.SubSocket, sendcan: messaging.PubSocket, que
                 print(f"Duplicate ECU address: {hex(msg.address)}")
             ecu_responses.add((msg.address, subaddr, msg.src))
   except Exception:
-    cloudlog.warning(f"ECU addr scan exception: {traceback.format_exc()}")
+    cloudlog.exception("ECU addr scan exception")
   return ecu_responses
 
 
@@ -71,6 +70,8 @@ if __name__ == "__main__":
 
   parser = argparse.ArgumentParser(description='Get addresses of all ECUs')
   parser.add_argument('--debug', action='store_true')
+  parser.add_argument('--bus', type=int, default=1)
+  parser.add_argument('--timeout', type=float, default=1.0)
   args = parser.parse_args()
 
   logcan = messaging.sub_sock('can')
@@ -79,7 +80,7 @@ if __name__ == "__main__":
   time.sleep(1.0)
 
   print("Getting ECU addresses ...")
-  ecu_addrs = get_all_ecu_addrs(logcan, sendcan, 1, debug=args.debug)
+  ecu_addrs = get_all_ecu_addrs(logcan, sendcan, args.bus, args.timeout, debug=args.debug)
 
   print()
   print("Found ECUs on addresses:")

selfdrive/car/ford/carcontroller.py

@@ -3,7 +3,7 @@ from cereal import car
 from common.numpy_fast import clip, interp
 from opendbc.can.packer import CANPacker
 from selfdrive.car.ford import fordcan
-from selfdrive.car.ford.values import CarControllerParams
+from selfdrive.car.ford.values import CANBUS, CarControllerParams
 
 VisualAlert = car.CarControl.HUDControl.VisualAlert
 
@@ -16,9 +16,9 @@ def apply_ford_steer_angle_limits(apply_angle, apply_angle_last, vEgo):
   apply_angle = clip(apply_angle, (apply_angle_last - max_angle_diff), (apply_angle_last + max_angle_diff))
 
   # absolute limit (LatCtlPath_An_Actl)
-  apply_path_angle = math.radians(apply_angle) / CarControllerParams.STEER_RATIO
+  apply_path_angle = math.radians(apply_angle) / CarControllerParams.LKAS_STEER_RATIO
-  apply_path_angle = clip(apply_path_angle, -0.4995, 0.5240)
+  apply_path_angle = clip(apply_path_angle, -0.5, 0.5235)
-  apply_angle = math.degrees(apply_path_angle) * CarControllerParams.STEER_RATIO
+  apply_angle = math.degrees(apply_path_angle) * CarControllerParams.LKAS_STEER_RATIO
 
   return apply_angle
 
@@ -47,40 +47,46 @@ class CarController:
     ### acc buttons ###
     if CC.cruiseControl.cancel:
       can_sends.append(fordcan.create_button_command(self.packer, CS.buttons_stock_values, cancel=True))
-    elif CC.cruiseControl.resume:
+      can_sends.append(fordcan.create_button_command(self.packer, CS.buttons_stock_values, cancel=True, bus=CANBUS.main))
+    elif CC.cruiseControl.resume and (self.frame % CarControllerParams.BUTTONS_STEP) == 0:
       can_sends.append(fordcan.create_button_command(self.packer, CS.buttons_stock_values, resume=True))
-
+      can_sends.append(fordcan.create_button_command(self.packer, CS.buttons_stock_values, resume=True, bus=CANBUS.main))
-    # if stock lane centering is active or in standby, toggle it off
+    # if stock lane centering isn't off, send a button press to toggle it off
     # the stock system checks for steering pressed, and eventually disengages cruise control
-    if (self.frame % 200) == 0 and CS.acc_tja_status_stock_values["Tja_D_Stat"] != 0:
+    elif CS.acc_tja_status_stock_values["Tja_D_Stat"] != 0 and (self.frame % CarControllerParams.ACC_UI_STEP) == 0:
       can_sends.append(fordcan.create_button_command(self.packer, CS.buttons_stock_values, tja_toggle=True))
 
 
     ### lateral control ###
     if CC.latActive:
+      lca_rq = 1
       apply_angle = apply_ford_steer_angle_limits(actuators.steeringAngleDeg, self.apply_angle_last, CS.out.vEgo)
     else:
-      apply_angle = CS.out.steeringAngleDeg
+      lca_rq = 0
+      apply_angle = 0.
 
     # send steering commands at 20Hz
     if (self.frame % CarControllerParams.LKAS_STEER_STEP) == 0:
-      lca_rq = 1 if CC.latActive else 0
-
       # use LatCtlPath_An_Actl to actuate steering
-      # path angle is the car wheel angle, not the steering wheel angle
+      path_angle = math.radians(apply_angle) / CarControllerParams.LKAS_STEER_RATIO
-      path_angle = math.radians(apply_angle) / CarControllerParams.STEER_RATIO
+
-
+      # set slower ramp type when small steering angle change
-      # ramp rate: 0=Slow, 1=Medium, 2=Fast, 3=Immediately
+      # 0=Slow, 1=Medium, 2=Fast, 3=Immediately
-      # TODO: try slower ramp speed when driver torque detected
+      steer_change = abs(CS.out.steeringAngleDeg - actuators.steeringAngleDeg)
+      if steer_change < 2.0:
+        ramp_type = 0
+      elif steer_change < 4.0:
+        ramp_type = 1
+      elif steer_change < 6.0:
+        ramp_type = 2
+      else:
         ramp_type = 3
       precision = 1  # 0=Comfortable, 1=Precise (the stock system always uses comfortable)
 
-      offset_roll_compensation_curvature = clip(self.VM.calc_curvature(0, CS.out.vEgo, -CS.yaw_data["VehYaw_W_Actl"]), -0.02, 0.02094)
-
       self.apply_angle_last = apply_angle
-      can_sends.append(fordcan.create_lka_command(self.packer, apply_angle, 0))
+      can_sends.append(fordcan.create_lka_command(self.packer, 0, 0))
       can_sends.append(fordcan.create_tja_command(self.packer, lca_rq, ramp_type, precision,
-                                                  0, path_angle, 0, offset_roll_compensation_curvature))
+                                                  0, path_angle, 0, 0))
 
 
     ### ui ###
@@ -99,7 +105,7 @@ class CarController:
     self.steer_alert_last = steer_alert
 
     new_actuators = actuators.copy()
-    new_actuators.steeringAngleDeg = apply_angle
+    new_actuators.steeringAngleDeg = self.apply_angle_last
 
     self.frame += 1
     return new_actuators, can_sends

selfdrive/car/ford/carstate.py

@@ -20,7 +20,7 @@ class CarState(CarStateBase):
     ret = car.CarState.new_message()
 
     # car speed
-    ret.vEgoRaw = cp.vl["EngVehicleSpThrottle2"]["Veh_V_ActlEng"] * CV.KPH_TO_MS
+    ret.vEgoRaw = cp.vl["BrakeSysFeatures"]["Veh_V_ActlBrk"] * CV.KPH_TO_MS
     ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
     ret.yawRate = cp.vl["Yaw_Data_FD1"]["VehYaw_W_Actl"]
     ret.standstill = cp.vl["DesiredTorqBrk"]["VehStop_D_Stat"] == 1
@@ -85,8 +85,6 @@ class CarState(CarStateBase):
     # Stock values from IPMA so that we can retain some stock functionality
     self.acc_tja_status_stock_values = cp_cam.vl["ACCDATA_3"]
     self.lkas_status_stock_values = cp_cam.vl["IPMA_Data"]
-    # Use stock sensor values
-    self.yaw_data = cp.vl["Yaw_Data_FD1"]
 
     return ret
 
@@ -94,7 +92,7 @@ class CarState(CarStateBase):
   def get_can_parser(CP):
     signals = [
       # sig_name, sig_address
-      ("Veh_V_ActlEng", "EngVehicleSpThrottle2"),            # ABS vehicle speed (kph)
+      ("Veh_V_ActlBrk", "BrakeSysFeatures"),                 # ABS vehicle speed (kph)
       ("VehYaw_W_Actl", "Yaw_Data_FD1"),                     # ABS vehicle yaw rate (rad/s)
       ("VehStop_D_Stat", "DesiredTorqBrk"),                  # ABS vehicle stopped
       ("PrkBrkStatus", "DesiredTorqBrk"),                    # ABS park brake status
@@ -156,7 +154,7 @@ class CarState(CarStateBase):
 
     checks = [
       # sig_address, frequency
-      ("EngVehicleSpThrottle2", 50),
+      ("BrakeSysFeatures", 50),
       ("Yaw_Data_FD1", 100),
       ("DesiredTorqBrk", 50),
       ("EngVehicleSpThrottle", 100),

selfdrive/car/ford/fordcan.py

@@ -8,8 +8,7 @@ def create_lka_command(packer, angle_deg: float, curvature: float):
   """
   Creates a CAN message for the Ford LKAS Command.
 
-  This command can apply "Lane Keeping Aid" manoeuvres, which are subject to the
+  This command can apply "Lane Keeping Aid" manoeuvres, which are subject to the PSCM lockout.
-  PSCM lockout.
 
   Frequency is 20Hz.
   """
@@ -30,12 +29,20 @@ def create_tja_command(packer, lca_rq: int, ramp_type: int, precision: int, path
   """
   Creates a CAN message for the Ford TJA/LCA Command.
 
-  This command can apply "Lane Centering" manoeuvres: continuous lane centering
+  This command can apply "Lane Centering" manoeuvres: continuous lane centering for traffic jam
-  for traffic jam assist and highway driving. It is not subject to the PSCM
+  assist and highway driving. It is not subject to the PSCM lockout.
-  lockout.
 
-  The PSCM should be configured to accept TJA/LCA commands before these
+  Ford lane centering command uses a third order polynomial to describe the road centerline. The
-  commands will be processed. This can be done using tools such as Forscan.
+  polynomial is defined by the following coefficients:
+    c0: lateral offset between the vehicle and the centerline
+    c1: heading angle between the vehicle and the centerline
+    c2: curvature of the centerline
+    c3: rate of change of curvature of the centerline
+  As the PSCM combines this information with other sensor data, such as the vehicle's yaw rate and
+  speed, the steering angle cannot be easily controlled.
+
+  The PSCM should be configured to accept TJA/LCA commands before these commands will be processed.
+  This can be done using tools such as Forscan.
 
   Frequency is 20Hz.
   """
@@ -47,7 +54,7 @@ def create_tja_command(packer, lca_rq: int, ramp_type: int, precision: int, path
     "LatCtlRampType_D_Rq": ramp_type,                       # Ramp speed: 0=Slow, 1=Medium, 2=Fast, 3=Immediate [0|3]
     "LatCtlPrecision_D_Rq": precision,                      # Precision: 0=Comfortable, 1=Precise, 2/3=NotUsed [0|3]
     "LatCtlPathOffst_L_Actl": path_offset,                  # Path offset [-5.12|5.11] meter
-    "LatCtlPath_An_Actl": path_angle,                       # Path angle [-0.4995|0.5240] radians
+    "LatCtlPath_An_Actl": path_angle,                       # Path angle [-0.5|0.5235] radians
     "LatCtlCurv_NoRate_Actl": curvature_rate,               # Curvature rate [-0.001024|0.00102375] 1/meter^2
     "LatCtlCurv_No_Actl": curvature,                        # Curvature [-0.02|0.02094] 1/meter
   }
@@ -108,8 +115,8 @@ def create_lkas_ui_command(packer, main_on: bool, enabled: bool, steer_alert: bo
 
 def create_acc_ui_command(packer, main_on: bool, enabled: bool, hud_control, stock_values: dict):
   """
-  Creates a CAN message for the Ford IPC adaptive cruise, forward collision
+  Creates a CAN message for the Ford IPC adaptive cruise, forward collision warning and traffic jam
-  warning and traffic jam assist status.
+  assist status.
 
   Stock functionality is maintained by passing through unmodified signals.
 
@@ -141,7 +148,7 @@ def create_acc_ui_command(packer, main_on: bool, enabled: bool, hud_control, sto
   return packer.make_can_msg("ACCDATA_3", CANBUS.main, values)
 
 
-def create_button_command(packer, stock_values: dict, cancel = False, resume = False, tja_toggle = False, bus = CANBUS.camera):
+def create_button_command(packer, stock_values: dict, cancel = False, resume = False, tja_toggle = False, bus: int = CANBUS.camera):
   """
   Creates a CAN message for the Ford SCCM buttons/switches.
 

selfdrive/car/ford/interface.py

@@ -5,8 +5,7 @@ from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness,
 from selfdrive.car.ford.values import CAR, Ecu, TransmissionType, GearShifter
 from selfdrive.car.interfaces import CarInterfaceBase
 
-
+CarParams = car.CarParams
-EventName = car.CarEvent.EventName
 
 
 class CarInterface(CarInterfaceBase):
@@ -19,10 +18,10 @@ class CarInterface(CarInterfaceBase):
 
     ret.carName = "ford"
     ret.dashcamOnly = True
-    ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.ford)]
+    ret.safetyConfigs = [get_safety_config(CarParams.SafetyModel.ford)]
 
     # Angle-based steering
-    ret.steerControlType = car.CarParams.SteerControlType.angle
+    ret.steerControlType = CarParams.SteerControlType.angle
     ret.steerActuatorDelay = 0.4
     ret.steerLimitTimer = 1.0
     tire_stiffness_factor = 1.0
@@ -43,7 +42,7 @@ class CarInterface(CarInterfaceBase):
       ret.mass = 1350 + STD_CARGO_KG
 
     else:
-      raise ValueError(f"Unsupported car: ${candidate}")
+      raise ValueError(f"Unsupported car: {candidate}")
 
     # Auto Transmission: 0x732 ECU or Gear_Shift_by_Wire_FD1
     found_ecus = [fw.ecu for fw in car_fw]

selfdrive/car/ford/values.py

@@ -1,4 +1,4 @@
-from collections import namedtuple
+from collections import defaultdict, namedtuple
 from dataclasses import dataclass
 from enum import Enum
 from typing import Dict, List, Union
@@ -22,19 +22,17 @@ class CarControllerParams:
   LKAS_UI_STEP = 100
   # Message: ACCDATA_3
   ACC_UI_STEP = 5
+  # Message: Steering_Data_FD1, but send twice as fast
+  BUTTONS_STEP = 10 / 2
 
-  STEER_RATIO = 2.75
+  LKAS_STEER_RATIO = 2.75       # Approximate ratio between LatCtlPath_An_Actl and steering angle in radians
-  STEER_DRIVER_ALLOWANCE = 0.8
+                                # TODO: remove this once we understand how the EPS calculates the steering angle better
+  STEER_DRIVER_ALLOWANCE = 0.8  # Driver intervention threshold in Nm
 
   RATE_LIMIT_UP = AngleRateLimit(speed_points=[0., 5., 15.], max_angle_diff_points=[5., .8, .15])
   RATE_LIMIT_DOWN = AngleRateLimit(speed_points=[0., 5., 15.], max_angle_diff_points=[5., 3.5, 0.4])
 
 
-class RADAR:
-  DELPHI_ESR = 'ford_fusion_2018_adas'
-  DELPHI_MRR = 'FORD_CADS'
-
-
 class CANBUS:
   main = 0
   radar = 1
@@ -47,6 +45,14 @@ class CAR:
   FOCUS_MK4 = "FORD FOCUS 4TH GEN"
 
 
+class RADAR:
+  DELPHI_ESR = 'ford_fusion_2018_adas'
+  DELPHI_MRR = 'FORD_CADS'
+
+
+DBC: Dict[str, Dict[str, str]] = defaultdict(lambda: dbc_dict("ford_lincoln_base_pt", RADAR.DELPHI_MRR))
+
+
 @dataclass
 class FordCarInfo(CarInfo):
   package: str = "Co-Pilot360 Assist+"
@@ -143,10 +149,3 @@ FW_VERSIONS = {
     ],
   },
 }
-
-
-DBC = {
-  CAR.ESCAPE_MK4: dbc_dict('ford_lincoln_base_pt', RADAR.DELPHI_MRR),
-  CAR.EXPLORER_MK6: dbc_dict('ford_lincoln_base_pt', RADAR.DELPHI_MRR),
-  CAR.FOCUS_MK4: dbc_dict('ford_lincoln_base_pt', RADAR.DELPHI_MRR),
-}

selfdrive/car/fw_query_definitions.py

@@ -2,7 +2,7 @@
 import capnp
 from dataclasses import dataclass, field
 import struct
-from typing import Dict, List
+from typing import Dict, List, Optional, Tuple
 
 import panda.python.uds as uds
 
@@ -64,3 +64,5 @@ class FwQueryConfig:
   requests: List[Request]
   # Overrides and removes from essential ecus for specific models and ecus (exact matching)
   non_essential_ecus: Dict[capnp.lib.capnp._EnumModule, List[str]] = field(default_factory=dict)
+  # Ecus added for data collection, not to be fingerprinted on
+  extra_ecus: List[Tuple[capnp.lib.capnp._EnumModule, int, Optional[int]]] = field(default_factory=list)

selfdrive/car/fw_versions.py

@@ -1,5 +1,4 @@
 #!/usr/bin/env python3
-import traceback
 from collections import defaultdict
 from typing import Any, Optional, Set, Tuple
 from tqdm import tqdm
@@ -214,6 +213,11 @@ def get_fw_versions_ordered(logcan, sendcan, ecu_rx_addrs, timeout=0.1, debug=Fa
 
 def get_fw_versions(logcan, sendcan, query_brand=None, extra=None, timeout=0.1, debug=False, progress=False):
   versions = VERSIONS.copy()
+
+  # Each brand can define extra ECUs to query for data collection
+  for brand, config in FW_QUERY_CONFIGS.items():
+    versions[brand]["debug"] = {ecu: [] for ecu in config.extra_ecus}
+
   if query_brand is not None:
     versions = {query_brand: versions[query_brand]}
 
@@ -254,23 +258,23 @@ def get_fw_versions(logcan, sendcan, query_brand=None, extra=None, timeout=0.1,
 
           if addrs:
             query = IsoTpParallelQuery(sendcan, logcan, r.bus, addrs, r.request, r.response, r.rx_offset, debug=debug)
-            for (addr, rx_addr), version in query.get_data(timeout).items():
+            for (tx_addr, sub_addr), version in query.get_data(timeout).items():
               f = car.CarParams.CarFw.new_message()
 
-              f.ecu = ecu_types.get((brand, addr[0], addr[1]), Ecu.unknown)
+              f.ecu = ecu_types.get((brand, tx_addr, sub_addr), Ecu.unknown)
               f.fwVersion = version
-              f.address = addr[0]
+              f.address = tx_addr
-              f.responseAddress = rx_addr
+              f.responseAddress = uds.get_rx_addr_for_tx_addr(tx_addr, r.rx_offset)
               f.request = r.request
               f.brand = brand
               f.bus = r.bus
 
-              if addr[1] is not None:
+              if sub_addr is not None:
-                f.subAddress = addr[1]
+                f.subAddress = sub_addr
 
               car_fw.append(f)
         except Exception:
-          cloudlog.warning(f"FW query exception: {traceback.format_exc()}")
+          cloudlog.exception("FW query exception")
 
   return car_fw
 
@@ -304,8 +308,8 @@ if __name__ == "__main__":
 
   t = time.time()
   print("Getting vin...")
-  addr, vin_rx_addr, vin = get_vin(logcan, sendcan, 1, retry=10, debug=args.debug)
+  vin_rx_addr, vin = get_vin(logcan, sendcan, 1, retry=10, debug=args.debug)
-  print(f'TX: {hex(addr)}, RX: {hex(vin_rx_addr)}, VIN: {vin}')
+  print(f'RX: {hex(vin_rx_addr)}, VIN: {vin}')
   print(f"Getting VIN took {time.time() - t:.3f} s")
   print()
 

selfdrive/car/gm/carcontroller.py

@@ -21,7 +21,8 @@ class CarController:
     self.frame = 0
     self.last_button_frame = 0
 
-    self.lka_steering_cmd_counter_last = -1
+    self.lka_steering_cmd_counter = 0
+    self.sent_lka_steering_cmd = False
     self.lka_icon_status_last = (False, False)
 
     self.params = CarControllerParams()
@@ -44,9 +45,14 @@ class CarController:
     # Steering (50Hz)
     # Avoid GM EPS faults when transmitting messages too close together: skip this transmit if we just received the
     # next Panda loopback confirmation in the current CS frame.
-    if CS.lka_steering_cmd_counter != self.lka_steering_cmd_counter_last:
+    if CS.loopback_lka_steering_cmd_updated:
-      self.lka_steering_cmd_counter_last = CS.lka_steering_cmd_counter
+      self.lka_steering_cmd_counter += 1
+      self.sent_lka_steering_cmd = True
     elif (self.frame % self.params.STEER_STEP) == 0:
+      # Initialize ASCMLKASteeringCmd counter using the camera
+      if not self.sent_lka_steering_cmd and self.CP.networkLocation == NetworkLocation.fwdCamera:
+        self.lka_steering_cmd_counter = CS.camera_lka_steering_cmd_counter + 1
+
       if CC.latActive:
         new_steer = int(round(actuators.steer * self.params.STEER_MAX))
         apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, self.params)
@@ -54,10 +60,7 @@ class CarController:
         apply_steer = 0
 
       self.apply_steer_last = apply_steer
-      # GM EPS faults on any gap in received message counters. To handle transient OP/Panda safety sync issues at the
+      idx = self.lka_steering_cmd_counter % 4
-      # moment of disengaging, increment the counter based on the last message known to pass Panda safety checks.
-      idx = (CS.lka_steering_cmd_counter + 1) % 4
-
       can_sends.append(gmcan.create_steering_control(self.packer_pt, CanBus.POWERTRAIN, apply_steer, idx, CC.latActive))
 
     if self.CP.openpilotLongitudinalControl:

selfdrive/car/gm/carstate.py

@@ -15,7 +15,8 @@ class CarState(CarStateBase):
     super().__init__(CP)
     can_define = CANDefine(DBC[CP.carFingerprint]["pt"])
     self.shifter_values = can_define.dv["ECMPRDNL2"]["PRNDL2"]
-    self.lka_steering_cmd_counter = 0
+    self.loopback_lka_steering_cmd_updated = False
+    self.camera_lka_steering_cmd_counter = 0
     self.buttons_counter = 0
 
   def update(self, pt_cp, cam_cp, loopback_cp):
@@ -25,6 +26,11 @@ class CarState(CarStateBase):
     self.cruise_buttons = pt_cp.vl["ASCMSteeringButton"]["ACCButtons"]
     self.buttons_counter = pt_cp.vl["ASCMSteeringButton"]["RollingCounter"]
 
+    # Variables used for avoiding LKAS faults
+    self.loopback_lka_steering_cmd_updated = len(loopback_cp.vl_all["ASCMLKASteeringCmd"]) > 0
+    if self.CP.networkLocation == NetworkLocation.fwdCamera:
+      self.camera_lka_steering_cmd_counter = cam_cp.vl["ASCMLKASteeringCmd"]["RollingCounter"]
+
     ret.wheelSpeeds = self.get_wheel_speeds(
       pt_cp.vl["EBCMWheelSpdFront"]["FLWheelSpd"],
       pt_cp.vl["EBCMWheelSpdFront"]["FRWheelSpd"],
@@ -40,9 +46,17 @@ class CarState(CarStateBase):
     else:
       ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(pt_cp.vl["ECMPRDNL2"]["PRNDL2"], None))
 
-    # Brake pedal's potentiometer returns near-zero reading even when pedal is not pressed.
+    # Some Volt 2016-17 have loose brake pedal push rod retainers which causes the ECM to believe
-    ret.brake = pt_cp.vl["EBCMBrakePedalPosition"]["BrakePedalPosition"] / 0xd0
+    # that the brake is being intermittently pressed without user interaction.
-    ret.brakePressed = pt_cp.vl["EBCMBrakePedalPosition"]["BrakePedalPosition"] >= 10
+    # To avoid a cruise fault we need to match the ECM's brake pressed signal and threshold
+    # https://static.nhtsa.gov/odi/tsbs/2017/MC-10137629-9999.pdf
+    ret.brake = pt_cp.vl["ECMAcceleratorPos"]["BrakePedalPos"]
+    if self.CP.networkLocation != NetworkLocation.fwdCamera:
+      ret.brakePressed = ret.brake >= 8
+    else:
+      # While car is braking, cancel button causes ECM to enter a soft disable state with a fault status.
+      # Match ECM threshold at a standstill to allow the camera to cancel earlier
+      ret.brakePressed = ret.brake >= 20
 
     # Regen braking is braking
     if self.CP.transmissionType == TransmissionType.direct:
@@ -56,7 +70,6 @@ class CarState(CarStateBase):
     ret.steeringTorque = pt_cp.vl["PSCMStatus"]["LKADriverAppldTrq"]
     ret.steeringTorqueEps = pt_cp.vl["PSCMStatus"]["LKATorqueDelivered"]
     ret.steeringPressed = abs(ret.steeringTorque) > STEER_THRESHOLD
-    self.lka_steering_cmd_counter = loopback_cp.vl["ASCMLKASteeringCmd"]["RollingCounter"]
 
     # 0 inactive, 1 active, 2 temporarily limited, 3 failed
     self.lkas_status = pt_cp.vl["PSCMStatus"]["LKATorqueDeliveredStatus"]
@@ -84,6 +97,9 @@ class CarState(CarStateBase):
     if self.CP.networkLocation == NetworkLocation.fwdCamera:
       ret.cruiseState.speed = cam_cp.vl["ASCMActiveCruiseControlStatus"]["ACCSpeedSetpoint"] * CV.KPH_TO_MS
 
+      ret.stockAeb = cam_cp.vl["AEBCmd"]["AEBCmdActive"] != 0
+      ret.stockFcw = cam_cp.vl["ASCMActiveCruiseControlStatus"]["FCWAlert"] != 0
+
     return ret
 
   @staticmethod
@@ -91,8 +107,17 @@ class CarState(CarStateBase):
     signals = []
     checks = []
     if CP.networkLocation == NetworkLocation.fwdCamera:
-      signals.append(("ACCSpeedSetpoint", "ASCMActiveCruiseControlStatus"))
+      signals += [
-      checks.append(("ASCMActiveCruiseControlStatus", 25))
+        ("AEBCmdActive", "AEBCmd"),
+        ("RollingCounter", "ASCMLKASteeringCmd"),
+        ("FCWAlert", "ASCMActiveCruiseControlStatus"),
+        ("ACCSpeedSetpoint", "ASCMActiveCruiseControlStatus"),
+      ]
+      checks += [
+        ("AEBCmd", 10),
+        ("ASCMLKASteeringCmd", 10),
+        ("ASCMActiveCruiseControlStatus", 25),
+      ]
 
     return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, CanBus.CAMERA)
 
@@ -100,7 +125,7 @@ class CarState(CarStateBase):
   def get_can_parser(CP):
     signals = [
       # sig_name, sig_address
-      ("BrakePedalPosition", "EBCMBrakePedalPosition"),
+      ("BrakePedalPos", "ECMAcceleratorPos"),
       ("FrontLeftDoor", "BCMDoorBeltStatus"),
       ("FrontRightDoor", "BCMDoorBeltStatus"),
       ("RearLeftDoor", "BCMDoorBeltStatus"),
@@ -141,7 +166,7 @@ class CarState(CarStateBase):
       ("ASCMSteeringButton", 33),
       ("ECMEngineStatus", 100),
       ("PSCMSteeringAngle", 100),
-      ("EBCMBrakePedalPosition", 100),
+      ("ECMAcceleratorPos", 80),
     ]
 
     if CP.transmissionType == TransmissionType.direct:
@@ -157,9 +182,7 @@ class CarState(CarStateBase):
     ]
 
     checks = [
-      ("ASCMLKASteeringCmd", 10), # 10 Hz is the stock inactive rate (every 100ms).
+      ("ASCMLKASteeringCmd", 0),
-      #                             While active 50 Hz (every 20 ms) is normal
-      #                             EPS will tolerate around 200ms when active before faulting
     ]
 
-    return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, CanBus.LOOPBACK)
+    return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, CanBus.LOOPBACK, enforce_checks=False)

selfdrive/car/gm/interface.py

@@ -212,7 +212,13 @@ class CarInterface(CarInterfaceBase):
     if ret.cruiseState.standstill:
       events.add(EventName.resumeRequired)
     if ret.vEgo < self.CP.minSteerSpeed:
-      events.add(car.CarEvent.EventName.belowSteerSpeed)
+      events.add(EventName.belowSteerSpeed)
+
+    if self.CP.networkLocation == NetworkLocation.fwdCamera and self.CP.pcmCruise:
+      # The ECM has a higher brake pressed threshold than the camera, causing an
+      # ACC fault when you engage at a stop with your foot partially on the brake
+      if ret.vEgoRaw < 0.1 and ret.brake < 20:
+        events.add(EventName.gmAccFaultedTemp)
 
     ret.events = events.to_msg()
 

selfdrive/car/gm/values.py

@@ -23,11 +23,12 @@ class CarControllerParams:
   ADAS_KEEPALIVE_STEP = 100
   CAMERA_KEEPALIVE_STEP = 100
 
-  # Volt gasbrake lookups
+  # Volt gas/brake lookups
-  # TODO: These values should be confirmed on non-Volt vehicles
+  # TODO: These values should be confirmed on non-Volt vehicles.
+  # MAX_GAS should achieve 2 m/s^2 and MAX_BRAKE with regen should achieve -4.0 m/s^2
   MAX_GAS = 3072  # Safety limit, not ACC max. Stock ACC >4096 from standstill.
   ZERO_GAS = 2048  # Coasting
-  MAX_BRAKE = 350  # ~ -3.5 m/s^2 with regen
+  MAX_BRAKE = 400  # ~ -4.0 m/s^2 with regen
   MAX_ACC_REGEN = 1404  # Max ACC regen is slightly less than max paddle regen
 
   # Allow small margin below -3.5 m/s^2 from ISO 15622:2018 since we
@@ -38,15 +39,14 @@ class CarControllerParams:
   ACCEL_MAX = 2.  # m/s^2
   ACCEL_MIN = -4.  # m/s^2
 
-  EV_GAS_LOOKUP_BP = [-1., 0., ACCEL_MAX]
-  EV_BRAKE_LOOKUP_BP = [ACCEL_MIN, -1.]
-
   # ICE has much less engine braking force compared to regen in EVs,
   # lower threshold removes some braking deadzone
   GAS_LOOKUP_BP = [-0.1, 0., ACCEL_MAX]
-  BRAKE_LOOKUP_BP = [ACCEL_MIN, -0.1]
+  EV_GAS_LOOKUP_BP = [-1., 0., ACCEL_MAX]
-
   GAS_LOOKUP_V = [MAX_ACC_REGEN, ZERO_GAS, MAX_GAS]
+
+  BRAKE_LOOKUP_BP = [ACCEL_MIN, -0.1]
+  EV_BRAKE_LOOKUP_BP = [ACCEL_MIN, -1.]
   BRAKE_LOOKUP_V = [MAX_BRAKE, 0.]
 
 
@@ -86,13 +86,13 @@ CAR_INFO: Dict[str, Union[GMCarInfo, List[GMCarInfo]]] = {
   CAR.ACADIA: GMCarInfo("GMC Acadia 2018", video_link="https://www.youtube.com/watch?v=0ZN6DdsBUZo"),
   CAR.BUICK_REGAL: GMCarInfo("Buick Regal Essence 2018"),
   CAR.ESCALADE_ESV: GMCarInfo("Cadillac Escalade ESV 2016", "Adaptive Cruise Control (ACC) & LKAS"),
-  CAR.BOLT_EV: GMCarInfo("Chevrolet Bolt EV 2022-23", "Adaptive Cruise Control (ACC)", footnotes=[], harness=Harness.gm),
+  CAR.BOLT_EV: GMCarInfo("Chevrolet Bolt EV 2022-23", footnotes=[], harness=Harness.gm),
-  CAR.BOLT_EUV: GMCarInfo("Chevrolet Bolt EUV 2022-23", "Premier or Premier Redline Trim without Super Cruise Package", video_link="https://youtu.be/xvwzGMUA210", footnotes=[], harness=Harness.gm),
+  CAR.BOLT_EUV: GMCarInfo("Chevrolet Bolt EUV 2022-23", "Premier or Premier Redline Trim without Super Cruise Package", "https://youtu.be/xvwzGMUA210", footnotes=[], harness=Harness.gm),
   CAR.SILVERADO: [
     GMCarInfo("Chevrolet Silverado 1500 2020-21", "Safety Package II", footnotes=[], harness=Harness.gm),
     GMCarInfo("GMC Sierra 1500 2020-21", "Driver Alert Package II", footnotes=[], harness=Harness.gm),
   ],
-  CAR.EQUINOX: GMCarInfo("Chevrolet Equinox 2019-22", "Adaptive Cruise Control (ACC)", footnotes=[], harness=Harness.gm),
+  CAR.EQUINOX: GMCarInfo("Chevrolet Equinox 2019-22", footnotes=[], harness=Harness.gm),
 }
 
 

selfdrive/car/honda/values.py

@@ -110,13 +110,13 @@ class HondaCarInfo(CarInfo):
 
 CAR_INFO: Dict[str, Optional[Union[HondaCarInfo, List[HondaCarInfo]]]] = {
   CAR.ACCORD: [
-    HondaCarInfo("Honda Accord 2018-22", "All", video_link="https://www.youtube.com/watch?v=mrUwlj3Mi58", min_steer_speed=3. * CV.MPH_TO_MS, harness=Harness.bosch_a),
+    HondaCarInfo("Honda Accord 2018-22", "All", "https://www.youtube.com/watch?v=mrUwlj3Mi58", min_steer_speed=3. * CV.MPH_TO_MS, harness=Harness.bosch_a),
     HondaCarInfo("Honda Inspire 2018", "All", min_steer_speed=3. * CV.MPH_TO_MS, harness=Harness.bosch_a),
   ],
   CAR.ACCORDH: HondaCarInfo("Honda Accord Hybrid 2018-22", "All", min_steer_speed=3. * CV.MPH_TO_MS, harness=Harness.bosch_a),
   CAR.CIVIC: HondaCarInfo("Honda Civic 2016-18", harness=Harness.nidec, video_link="https://youtu.be/-IkImTe1NYE"),
   CAR.CIVIC_BOSCH: [
-    HondaCarInfo("Honda Civic 2019-21", "All", video_link="https://www.youtube.com/watch?v=4Iz1Mz5LGF8", footnotes=[Footnote.CIVIC_DIESEL], min_steer_speed=2. * CV.MPH_TO_MS, harness=Harness.bosch_a),
+    HondaCarInfo("Honda Civic 2019-21", "All", "https://www.youtube.com/watch?v=4Iz1Mz5LGF8", [Footnote.CIVIC_DIESEL], min_steer_speed=2. * CV.MPH_TO_MS, harness=Harness.bosch_a),
     HondaCarInfo("Honda Civic Hatchback 2017-21", harness=Harness.bosch_a),
   ],
   CAR.CIVIC_BOSCH_DIESEL: None,  # same platform

selfdrive/car/hyundai/carcontroller.py

@@ -10,6 +10,12 @@ from selfdrive.car.hyundai.values import HyundaiFlags, Buttons, CarControllerPar
 VisualAlert = car.CarControl.HUDControl.VisualAlert
 LongCtrlState = car.CarControl.Actuators.LongControlState
 
+# EPS faults if you apply torque while the steering angle is above 90 degrees for more than 1 second
+# All slightly below EPS thresholds to avoid fault
+MAX_ANGLE = 85
+MAX_ANGLE_FRAMES = 89
+MAX_ANGLE_CONSECUTIVE_FRAMES = 2
+
 
 def process_hud_alert(enabled, fingerprint, hud_control):
   sys_warning = (hud_control.visualAlert in (VisualAlert.steerRequired, VisualAlert.ldw))
@@ -40,22 +46,21 @@ class CarController:
     self.CP = CP
     self.params = CarControllerParams(CP)
     self.packer = CANPacker(dbc_name)
+    self.angle_limit_counter = 0
     self.frame = 0
 
     self.apply_steer_last = 0
     self.car_fingerprint = CP.carFingerprint
     self.last_button_frame = 0
-    self.accel = 0
 
   def update(self, CC, CS):
     actuators = CC.actuators
     hud_control = CC.hudControl
 
-    # Steering Torque
+    # steering torque
-
-    # These cars have significantly more torque than most HKG.  Limit to 70% of max.
     steer = actuators.steer
     if self.CP.carFingerprint in (CAR.KONA, CAR.KONA_EV, CAR.KONA_HEV, CAR.KONA_EV_2022):
+      # these cars have significantly more torque than most HKG; limit to 70% of max
       steer = clip(steer, -0.7, 0.7)
     new_steer = int(round(steer * self.params.STEER_MAX))
     apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, self.params)
@@ -65,29 +70,54 @@ class CarController:
 
     self.apply_steer_last = apply_steer
 
+    # accel + longitudinal
+    accel = clip(actuators.accel, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX)
+    stopping = actuators.longControlState == LongCtrlState.stopping
+    set_speed_in_units = hud_control.setSpeed * (CV.MS_TO_KPH if CS.is_metric else CV.MS_TO_MPH)
+
+    # HUD messages
     sys_warning, sys_state, left_lane_warning, right_lane_warning = process_hud_alert(CC.enabled, self.car_fingerprint,
                                                                                       hud_control)
 
     can_sends = []
 
+    # *** common hyundai stuff ***
+
+    # tester present - w/ no response (keeps relevant ECU disabled)
+    if self.frame % 100 == 0 and self.CP.openpilotLongitudinalControl:
+      addr, bus = 0x7d0, 0
+      if self.CP.flags & HyundaiFlags.CANFD_HDA2.value:
+        addr, bus = 0x730, 5
+      can_sends.append([addr, 0, b"\x02\x3E\x80\x00\x00\x00\x00\x00", bus])
+
+    # CAN-FD platforms
     if self.CP.carFingerprint in CANFD_CAR:
+      hda2 = self.CP.flags & HyundaiFlags.CANFD_HDA2
+      hda2_long = hda2 and self.CP.openpilotLongitudinalControl
+
       # steering control
-      can_sends.append(hyundaicanfd.create_lkas(self.packer, self.CP, CC.enabled, CC.latActive, apply_steer))
+      can_sends.extend(hyundaicanfd.create_steering_messages(self.packer, self.CP, CC.enabled, CC.latActive, apply_steer))
 
-      # block LFA on HDA2
+      # disable LFA on HDA2
-      if self.frame % 5 == 0 and (self.CP.flags & HyundaiFlags.CANFD_HDA2):
+      if self.frame % 5 == 0 and hda2:
         can_sends.append(hyundaicanfd.create_cam_0x2a4(self.packer, CS.cam_0x2a4))
 
       # LFA and HDA icons
-      if self.frame % 2 == 0 and not (self.CP.flags & HyundaiFlags.CANFD_HDA2):
+      if self.frame % 5 == 0 and (not hda2 or hda2_long):
-        can_sends.append(hyundaicanfd.create_lfahda_cluster(self.packer, CC.enabled))
+        can_sends.append(hyundaicanfd.create_lfahda_cluster(self.packer, self.CP, CC.enabled))
 
+      if self.CP.openpilotLongitudinalControl:
+        can_sends.extend(hyundaicanfd.create_adrv_messages(self.packer, self.frame))
+        if self.frame % 2 == 0:
+          can_sends.append(hyundaicanfd.create_acc_control(self.packer, self.CP, CC.enabled, accel, stopping, CC.cruiseControl.override,
+                                                           set_speed_in_units))
+      else:
         # button presses
         if (self.frame - self.last_button_frame) * DT_CTRL > 0.25:
           # cruise cancel
           if CC.cruiseControl.cancel:
             if self.CP.flags & HyundaiFlags.CANFD_ALT_BUTTONS:
-            can_sends.append(hyundaicanfd.create_cruise_info(self.packer, CS.cruise_info_copy, True))
+              can_sends.append(hyundaicanfd.create_acc_cancel(self.packer, self.CP, CS.cruise_info))
               self.last_button_frame = self.frame
             else:
               for _ in range(20):
@@ -101,14 +131,21 @@ class CarController:
                 can_sends.append(hyundaicanfd.create_buttons(self.packer, CS.buttons_counter+1, Buttons.RES_ACCEL))
               self.last_button_frame = self.frame
     else:
+      # Count up to MAX_ANGLE_FRAMES, at which point we need to cut torque to avoid a steering fault
+      if CC.latActive and abs(CS.out.steeringAngleDeg) >= MAX_ANGLE:
+        self.angle_limit_counter += 1
+      else:
+        self.angle_limit_counter = 0
 
-      # tester present - w/ no response (keeps radar disabled)
+      # Cut steer actuation bit for two frames and hold torque with induced temporary fault
-      if self.CP.openpilotLongitudinalControl:
+      torque_fault = CC.latActive and self.angle_limit_counter > MAX_ANGLE_FRAMES
-        if self.frame % 100 == 0:
+      lat_active = CC.latActive and not torque_fault
-          can_sends.append([0x7D0, 0, b"\x02\x3E\x80\x00\x00\x00\x00\x00", 0])
+
+      if self.angle_limit_counter >= MAX_ANGLE_FRAMES + MAX_ANGLE_CONSECUTIVE_FRAMES:
+        self.angle_limit_counter = 0
 
-      can_sends.append(hyundaican.create_lkas11(self.packer, self.frame, self.car_fingerprint, apply_steer, CC.latActive,
+      can_sends.append(hyundaican.create_lkas11(self.packer, self.frame, self.car_fingerprint, apply_steer, lat_active,
-                                     CS.lkas11, sys_warning, sys_state, CC.enabled,
+                                                torque_fault, CS.lkas11, sys_warning, sys_state, CC.enabled,
                                                 hud_control.leftLaneVisible, hud_control.rightLaneVisible,
                                                 left_lane_warning, right_lane_warning))
 
@@ -123,18 +160,10 @@ class CarController:
             self.last_button_frame = self.frame
 
       if self.frame % 2 == 0 and self.CP.openpilotLongitudinalControl:
-        accel = actuators.accel
+        # TODO: unclear if this is needed
-
-        #TODO unclear if this is needed
         jerk = 3.0 if actuators.longControlState == LongCtrlState.pid else 1.0
-
-        accel = clip(accel, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX)
-
-        stopping = actuators.longControlState == LongCtrlState.stopping
-        set_speed_in_units = hud_control.setSpeed * (CV.MS_TO_MPH if CS.clu11["CF_Clu_SPEED_UNIT"] == 1 else CV.MS_TO_KPH)
         can_sends.extend(hyundaican.create_acc_commands(self.packer, CC.enabled, accel, jerk, int(self.frame / 2),
-                                                        hud_control.leadVisible, set_speed_in_units, stopping, CS.out.gasPressed))
+                                                        hud_control.leadVisible, set_speed_in_units, stopping, CC.cruiseControl.override))
-        self.accel = accel
 
       # 20 Hz LFA MFA message
       if self.frame % 5 == 0 and self.car_fingerprint in (CAR.SONATA, CAR.PALISADE, CAR.IONIQ, CAR.KIA_NIRO_EV, CAR.KIA_NIRO_HEV_2021,
@@ -153,7 +182,7 @@ class CarController:
 
     new_actuators = actuators.copy()
     new_actuators.steer = apply_steer / self.params.STEER_MAX
-    new_actuators.accel = self.accel
+    new_actuators.accel = accel
 
     self.frame += 1
     return new_actuators, can_sends

selfdrive/car/hyundai/carstate.py

@@ -30,10 +30,12 @@ class CarState(CarStateBase):
     else:  # preferred and elect gear methods use same definition
       self.shifter_values = can_define.dv["LVR12"]["CF_Lvr_Gear"]
 
+    self.is_metric = False
     self.brake_error = False
-    self.park_brake = False
     self.buttons_counter = 0
 
+    self.cruise_info = {}
+
     # On some cars, CLU15->CF_Clu_VehicleSpeed can oscillate faster than the dash updates. Sample at 5 Hz
     self.cluster_speed = 0
     self.cluster_speed_counter = CLUSTER_SAMPLE_RATE
@@ -46,8 +48,8 @@ class CarState(CarStateBase):
 
     ret = car.CarState.new_message()
     cp_cruise = cp_cam if self.CP.carFingerprint in CAMERA_SCC_CAR else cp
-    is_metric = cp.vl["CLU11"]["CF_Clu_SPEED_UNIT"] == 0
+    self.is_metric = cp.vl["CLU11"]["CF_Clu_SPEED_UNIT"] == 0
-    speed_conv = CV.KPH_TO_MS if is_metric else CV.MPH_TO_MS
+    speed_conv = CV.KPH_TO_MS if self.is_metric else CV.MPH_TO_MS
 
     ret.doorOpen = any([cp.vl["CGW1"]["CF_Gway_DrvDrSw"], cp.vl["CGW1"]["CF_Gway_AstDrSw"],
                         cp.vl["CGW2"]["CF_Gway_RLDrSw"], cp.vl["CGW2"]["CF_Gway_RRDrSw"]])
@@ -70,7 +72,7 @@ class CarState(CarStateBase):
       self.cluster_speed_counter = 0
 
       # mimic how dash converts to imperial
-      if not is_metric:
+      if not self.is_metric:
         self.cluster_speed = math.floor(self.cluster_speed * CV.KPH_TO_MPH + CV.KPH_TO_MPH)
 
     ret.vEgoCluster = self.cluster_speed * speed_conv
@@ -127,12 +129,12 @@ class CarState(CarStateBase):
     ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(gear))
 
     if not self.CP.openpilotLongitudinalControl:
-      if self.CP.carFingerprint in FEATURES["use_fca"]:
+      aeb_src = "FCA11" if self.CP.carFingerprint in FEATURES["use_fca"] else "SCC12"
-        ret.stockAeb = cp_cruise.vl["FCA11"]["FCA_CmdAct"] != 0
+      aeb_sig = "FCA_CmdAct" if self.CP.carFingerprint in FEATURES["use_fca"] else "AEB_CmdAct"
-        ret.stockFcw = cp_cruise.vl["FCA11"]["CF_VSM_Warn"] == 2
+      aeb_warning = cp_cruise.vl[aeb_src]["CF_VSM_Warn"] != 0
-      else:
+      aeb_braking = cp_cruise.vl[aeb_src]["CF_VSM_DecCmdAct"] != 0 or cp_cruise.vl[aeb_src][aeb_sig] != 0
-        ret.stockAeb = cp_cruise.vl["SCC12"]["AEB_CmdAct"] != 0
+      ret.stockFcw = aeb_warning and not aeb_braking
-        ret.stockFcw = cp_cruise.vl["SCC12"]["CF_VSM_Warn"] == 2
+      ret.stockAeb = aeb_warning and aeb_braking
 
     if self.CP.enableBsm:
       ret.leftBlindspot = cp.vl["LCA11"]["CF_Lca_IndLeft"] != 0
@@ -152,9 +154,9 @@ class CarState(CarStateBase):
   def update_canfd(self, cp, cp_cam):
     ret = car.CarState.new_message()
 
-    if self.CP.flags & HyundaiFlags.CANFD_HDA2:
+    if self.CP.carFingerprint in EV_CAR:
       ret.gas = cp.vl["ACCELERATOR"]["ACCELERATOR_PEDAL"] / 255.
-    else:
+    elif self.CP.carFingerprint in HYBRID_CAR:
       ret.gas = cp.vl["ACCELERATOR_ALT"]["ACCELERATOR_PEDAL"] / 1023.
     ret.gasPressed = ret.gas > 1e-5
     ret.brakePressed = cp.vl["BRAKE"]["BRAKE_PRESSED"] == 1
@@ -181,22 +183,26 @@ class CarState(CarStateBase):
     ret.steeringTorque = cp.vl["MDPS"]["STEERING_COL_TORQUE"]
     ret.steeringTorqueEps = cp.vl["MDPS"]["STEERING_OUT_TORQUE"]
     ret.steeringPressed = abs(ret.steeringTorque) > self.params.STEER_THRESHOLD
+    ret.steerFaultTemporary = cp.vl["MDPS"]["LKA_FAULT"] != 0
 
     ret.leftBlinker, ret.rightBlinker = self.update_blinker_from_lamp(50, cp.vl["BLINKERS"]["LEFT_LAMP"],
                                                                       cp.vl["BLINKERS"]["RIGHT_LAMP"])
 
     ret.cruiseState.available = True
     ret.cruiseState.enabled = cp.vl["SCC1"]["CRUISE_ACTIVE"] == 1
+    self.is_metric = cp.vl["CLUSTER_INFO"]["DISTANCE_UNIT"] != 1
+    if not self.CP.openpilotLongitudinalControl:
+      speed_factor = CV.KPH_TO_MS if self.is_metric else CV.MPH_TO_MS
       cp_cruise_info = cp if self.CP.flags & HyundaiFlags.CANFD_HDA2 else cp_cam
-    speed_factor = CV.MPH_TO_MS if cp.vl["CLUSTER_INFO"]["DISTANCE_UNIT"] == 1 else CV.KPH_TO_MS
       ret.cruiseState.speed = cp_cruise_info.vl["CRUISE_INFO"]["SET_SPEED"] * speed_factor
       ret.cruiseState.standstill = cp_cruise_info.vl["CRUISE_INFO"]["CRUISE_STANDSTILL"] == 1
+      self.cruise_info = copy.copy(cp_cruise_info.vl["CRUISE_INFO"])
 
     cruise_btn_msg = "CRUISE_BUTTONS_ALT" if self.CP.flags & HyundaiFlags.CANFD_ALT_BUTTONS else "CRUISE_BUTTONS"
+    self.prev_cruise_buttons = self.cruise_buttons[-1]
     self.cruise_buttons.extend(cp.vl_all[cruise_btn_msg]["CRUISE_BUTTONS"])
     self.main_buttons.extend(cp.vl_all[cruise_btn_msg]["ADAPTIVE_CRUISE_MAIN_BTN"])
     self.buttons_counter = cp.vl[cruise_btn_msg]["COUNTER"]
-    self.cruise_info_copy = copy.copy(cp_cruise_info.vl["CRUISE_INFO"])
 
     if self.CP.flags & HyundaiFlags.CANFD_HDA2:
       self.cam_0x2a4 = copy.copy(cp_cam.vl["CAM_0x2a4"])
@@ -295,12 +301,14 @@ class CarState(CarStateBase):
         signals += [
           ("FCA_CmdAct", "FCA11"),
           ("CF_VSM_Warn", "FCA11"),
+          ("CF_VSM_DecCmdAct", "FCA11"),
         ]
         checks.append(("FCA11", 50))
       else:
         signals += [
           ("AEB_CmdAct", "SCC12"),
           ("CF_VSM_Warn", "SCC12"),
+          ("CF_VSM_DecCmdAct", "SCC12"),
         ]
 
     if CP.enableBsm:
@@ -384,12 +392,14 @@ class CarState(CarStateBase):
         signals += [
           ("FCA_CmdAct", "FCA11"),
           ("CF_VSM_Warn", "FCA11"),
+          ("CF_VSM_DecCmdAct", "FCA11"),
         ]
         checks.append(("FCA11", 50))
       else:
         signals += [
           ("AEB_CmdAct", "SCC12"),
           ("CF_VSM_Warn", "SCC12"),
+          ("CF_VSM_DecCmdAct", "SCC12"),
         ]
 
     return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 2)
@@ -411,6 +421,7 @@ class CarState(CarStateBase):
       ("STEERING_ANGLE", "STEERING_SENSORS"),
       ("STEERING_COL_TORQUE", "MDPS"),
       ("STEERING_OUT_TORQUE", "MDPS"),
+      ("LKA_FAULT", "MDPS"),
 
       ("CRUISE_ACTIVE", "SCC1"),
       ("COUNTER", cruise_btn_msg),
@@ -439,18 +450,23 @@ class CarState(CarStateBase):
       ("DOORS_SEATBELTS", 4),
     ]
 
-    if CP.flags & HyundaiFlags.CANFD_HDA2:
+    if CP.flags & HyundaiFlags.CANFD_HDA2 and not CP.openpilotLongitudinalControl:
       signals += [
-        ("ACCELERATOR_PEDAL", "ACCELERATOR"),
-        ("GEAR", "ACCELERATOR"),
         ("SET_SPEED", "CRUISE_INFO"),
         ("CRUISE_STANDSTILL", "CRUISE_INFO"),
       ]
       checks += [
         ("CRUISE_INFO", 50),
+      ]
+
+    if CP.carFingerprint in EV_CAR:
+      signals += [
+        ("ACCELERATOR_PEDAL", "ACCELERATOR"),
+      ]
+      checks += [
         ("ACCELERATOR", 100),
       ]
-    else:
+    elif CP.carFingerprint in HYBRID_CAR:
       signals += [
         ("ACCELERATOR_PEDAL", "ACCELERATOR_ALT"),
       ]
@@ -473,17 +489,14 @@ class CarState(CarStateBase):
         ("CRUISE_MAIN", "CRUISE_INFO"),
         ("CRUISE_STATUS", "CRUISE_INFO"),
         ("CRUISE_INACTIVE", "CRUISE_INFO"),
-        ("NEW_SIGNAL_2", "CRUISE_INFO"),
+        ("ZEROS_9", "CRUISE_INFO"),
         ("CRUISE_STANDSTILL", "CRUISE_INFO"),
-        ("NEW_SIGNAL_3", "CRUISE_INFO"),
+        ("ZEROS_5", "CRUISE_INFO"),
-        ("BYTE11", "CRUISE_INFO"),
+        ("DISTANCE_SETTING", "CRUISE_INFO"),
         ("SET_SPEED", "CRUISE_INFO"),
         ("NEW_SIGNAL_4", "CRUISE_INFO"),
       ]
 
-      signals += [(f"BYTE{i}", "CRUISE_INFO") for i in range(3, 7)]
-      signals += [(f"BYTE{i}", "CRUISE_INFO") for i in range(13, 31)]
-
       checks = [
         ("CRUISE_INFO", 50),
       ]

selfdrive/car/hyundai/hyundaican.py

@@ -4,7 +4,7 @@ from selfdrive.car.hyundai.values import CAR, CHECKSUM, CAMERA_SCC_CAR
 hyundai_checksum = crcmod.mkCrcFun(0x11D, initCrc=0xFD, rev=False, xorOut=0xdf)
 
 def create_lkas11(packer, frame, car_fingerprint, apply_steer, steer_req,
-                  lkas11, sys_warning, sys_state, enabled,
+                  torque_fault, lkas11, sys_warning, sys_state, enabled,
                   left_lane, right_lane,
                   left_lane_depart, right_lane_depart):
   values = lkas11
@@ -14,6 +14,7 @@ def create_lkas11(packer, frame, car_fingerprint, apply_steer, steer_req,
   values["CF_Lkas_LdwsRHWarning"] = right_lane_depart
   values["CR_Lkas_StrToqReq"] = apply_steer
   values["CF_Lkas_ActToi"] = steer_req
+  values["CF_Lkas_ToiFlt"] = torque_fault  # seems to allow actuation on CR_Lkas_StrToqReq
   values["CF_Lkas_MsgCount"] = frame % 0x10
 
   if car_fingerprint in (CAR.SONATA, CAR.PALISADE, CAR.KIA_NIRO_EV, CAR.KIA_NIRO_HEV_2021, CAR.SANTA_FE,
@@ -38,7 +39,7 @@ def create_lkas11(packer, frame, car_fingerprint, apply_steer, steer_req,
     values["CF_Lkas_SysWarning"] = 4 if sys_warning else 0
 
   # Likely cars lacking the ability to show individual lane lines in the dash
-  elif car_fingerprint in (CAR.KIA_OPTIMA, CAR.KIA_OPTIMA_2019):
+  elif car_fingerprint in (CAR.KIA_OPTIMA_G4, CAR.KIA_OPTIMA_G4_FL):
     # SysWarning 4 = keep hands on wheel + beep
     values["CF_Lkas_SysWarning"] = 4 if sys_warning else 0
 
@@ -94,7 +95,7 @@ def create_lfahda_mfc(packer, enabled, hda_set_speed=0):
   }
   return packer.make_can_msg("LFAHDA_MFC", 0, values)
 
-def create_acc_commands(packer, enabled, accel, upper_jerk, idx, lead_visible, set_speed, stopping, gas_pressed):
+def create_acc_commands(packer, enabled, accel, upper_jerk, idx, lead_visible, set_speed, stopping, long_override):
   commands = []
 
   scc11_values = {
@@ -111,7 +112,7 @@ def create_acc_commands(packer, enabled, accel, upper_jerk, idx, lead_visible, s
   commands.append(packer.make_can_msg("SCC11", 0, scc11_values))
 
   scc12_values = {
-    "ACCMode": 2 if enabled and gas_pressed else 1 if enabled else 0,
+    "ACCMode": 2 if enabled and long_override else 1 if enabled else 0,
     "StopReq": 1 if stopping else 0,
     "aReqRaw": accel,
     "aReqValue": accel,  # stock ramps up and down respecting jerk limit until it reaches aReqRaw
@@ -127,23 +128,21 @@ def create_acc_commands(packer, enabled, accel, upper_jerk, idx, lead_visible, s
     "ComfortBandLower": 0.0, # stock usually is 0 but sometimes uses higher values
     "JerkUpperLimit": upper_jerk, # stock usually is 1.0 but sometimes uses higher values
     "JerkLowerLimit": 5.0, # stock usually is 0.5 but sometimes uses higher values
-    "ACCMode": 2 if enabled and gas_pressed else 1 if enabled else 4, # stock will always be 4 instead of 0 after first disengage
+    "ACCMode": 2 if enabled and long_override else 1 if enabled else 4, # stock will always be 4 instead of 0 after first disengage
     "ObjGap": 2 if lead_visible else 0, # 5: >30, m, 4: 25-30 m, 3: 20-25 m, 2: < 20 m, 0: no lead
   }
   commands.append(packer.make_can_msg("SCC14", 0, scc14_values))
 
+  # note that some vehicles most likely have an alternate checksum/counter definition
+  # https://github.com/commaai/opendbc/commit/9ddcdb22c4929baf310295e832668e6e7fcfa602
   fca11_values = {
-    # seems to count 2,1,0,3,2,1,0,3,2,1,0,3,2,1,0,repeat...
+    "CR_FCA_Alive": idx % 0xF,
-    # (where first value is aligned to Supplemental_Counter == 0)
-    # test: [(idx % 0xF, -((idx % 0xF) + 2) % 4) for idx in range(0x14)]
-    "CR_FCA_Alive": ((-((idx % 0xF) + 2) % 4) << 2) + 1,
-    "Supplemental_Counter": idx % 0xF,
     "PAINT1_Status": 1,
     "FCA_DrvSetStatus": 1,
     "FCA_Status": 1, # AEB disabled
   }
   fca11_dat = packer.make_can_msg("FCA11", 0, fca11_values)[2]
-  fca11_values["CR_FCA_ChkSum"] = 0x10 - sum(sum(divmod(i, 16)) for i in fca11_dat) % 0x10
+  fca11_values["CR_FCA_ChkSum"] = hyundai_checksum(fca11_dat[:7])
   commands.append(packer.make_can_msg("FCA11", 0, fca11_values))
 
   return commands

selfdrive/car/hyundai/hyundaicanfd.py

@@ -1,7 +1,17 @@
 from selfdrive.car.hyundai.values import HyundaiFlags
 
 
-def create_lkas(packer, CP, enabled, lat_active, apply_steer):
+def get_e_can_bus(CP):
+  # On the CAN-FD platforms, the LKAS camera is on both A-CAN and E-CAN. HDA2 cars
+  # have a a different harness than the HDA1 and non-HDA variants in order to split
+  # a different bus, since the steering is done by different ECUs.
+  return 5 if CP.flags & HyundaiFlags.CANFD_HDA2 else 4
+
+
+def create_steering_messages(packer, CP, enabled, lat_active, apply_steer):
+
+  ret = []
+
   values = {
     "LKA_MODE": 2,
     "LKA_ICON": 2 if enabled else 1,
@@ -14,8 +24,14 @@ def create_lkas(packer, CP, enabled, lat_active, apply_steer):
     "NEW_SIGNAL_2": 0,
   }
 
-  msg = "LKAS" if CP.flags & HyundaiFlags.CANFD_HDA2 else "LFA"
+  if CP.flags & HyundaiFlags.CANFD_HDA2:
-  return packer.make_can_msg(msg, 4, values)
+    if CP.openpilotLongitudinalControl:
+      ret.append(packer.make_can_msg("LFA", 5, values))
+    ret.append(packer.make_can_msg("LKAS", 4, values))
+  else:
+    ret.append(packer.make_can_msg("LFA", 4, values))
+
+  return ret
 
 def create_cam_0x2a4(packer, camera_values):
   camera_values.update({
@@ -31,16 +47,98 @@ def create_buttons(packer, cnt, btn):
   }
   return packer.make_can_msg("CRUISE_BUTTONS", 5, values)
 
-def create_cruise_info(packer, cruise_info_copy, cancel):
+def create_acc_cancel(packer, CP, cruise_info_copy):
   values = cruise_info_copy
-  if cancel:
+  values.update({
-    values["CRUISE_STATUS"] = 0
+    "CRUISE_STATUS": 0,
-    values["CRUISE_INACTIVE"] = 1
+    "CRUISE_INACTIVE": 1,
-  return packer.make_can_msg("CRUISE_INFO", 4, values)
+  })
+  return packer.make_can_msg("CRUISE_INFO", get_e_can_bus(CP), values)
 
-def create_lfahda_cluster(packer, enabled):
+def create_lfahda_cluster(packer, CP, enabled):
   values = {
     "HDA_ICON": 1 if enabled else 0,
     "LFA_ICON": 2 if enabled else 0,
   }
-  return packer.make_can_msg("LFAHDA_CLUSTER", 4, values)
+  return packer.make_can_msg("LFAHDA_CLUSTER", get_e_can_bus(CP), values)
+
+
+def create_acc_control(packer, CP, enabled, accel, stopping, gas_override, set_speed):
+  cruise_status = 0 if not enabled else (4 if gas_override else 2)
+  if not enabled or gas_override:
+    accel = 0
+  values = {
+    "CRUISE_STATUS": cruise_status,
+    "CRUISE_INACTIVE": 0 if enabled else 1,
+    "CRUISE_MAIN": 1,
+    "CRUISE_STANDSTILL": 0,
+    "STOP_REQ": 1 if stopping else 0,
+    "ACCEL_REQ": accel,
+    "ACCEL_REQ2": accel,
+    "SET_SPEED": set_speed,
+    "DISTANCE_SETTING": 4,
+
+    "ACC_ObjDist": 1,
+    "ObjValid": 1,
+    "OBJ_STATUS": 2,
+    "SET_ME_2": 0x2,
+    "SET_ME_3": 0x3,
+    "SET_ME_TMP_64": 0x64,
+
+    "NEW_SIGNAL_9": 2,
+    "NEW_SIGNAL_10": 4,
+  }
+
+  return packer.make_can_msg("CRUISE_INFO", get_e_can_bus(CP), values)
+
+
+
+def create_adrv_messages(packer, frame):
+  # messages needed to car happy after disabling
+  # the ADAS Driving ECU to do longitudinal control
+
+  ret = []
+
+  values = {
+  }
+  ret.append(packer.make_can_msg("ADRV_0x51", 4, values))
+
+  if frame % 2 == 0:
+    values = {
+      'AEB_SETTING': 0x1,  # show AEB disabled icon
+      'SET_ME_2': 0x2,
+      'SET_ME_FF': 0xff,
+      'SET_ME_FC': 0xfc,
+      'SET_ME_9': 0x9,
+    }
+    ret.append(packer.make_can_msg("ADRV_0x160", 5, values))
+
+  if frame % 5 == 0:
+    values = {
+      'SET_ME_1C': 0x1c,
+      'SET_ME_FF': 0xff,
+      'SET_ME_TMP_F': 0xf,
+      'SET_ME_TMP_F_2': 0xf,
+    }
+    ret.append(packer.make_can_msg("ADRV_0x1ea", 5, values))
+
+    values = {
+      'SET_ME_E1': 0xe1,
+      'SET_ME_3A': 0x3a,
+    }
+    ret.append(packer.make_can_msg("ADRV_0x200", 5, values))
+
+  if frame % 20 == 0:
+    values = {
+      'SET_ME_15': 0x15,
+    }
+    ret.append(packer.make_can_msg("ADRV_0x345", 5, values))
+
+  if frame % 100 == 0:
+    values = {
+      'SET_ME_22': 0x22,
+      'SET_ME_41': 0x41,
+    }
+    ret.append(packer.make_can_msg("ADRV_0x1da", 5, values))
+
+  return ret

selfdrive/car/hyundai/interface.py

@@ -27,31 +27,24 @@ class CarInterface(CarInterfaceBase):
     ret.carName = "hyundai"
     ret.radarOffCan = RADAR_START_ADDR not in fingerprint[1] or DBC[ret.carFingerprint]["radar"] is None
 
-    # WARNING: disabling radar also disables AEB (and we show the same warning on the instrument cluster as if you manually disabled AEB)
-    ret.experimentalLongitudinalAvailable = candidate not in (LEGACY_SAFETY_MODE_CAR | CAMERA_SCC_CAR | CANFD_CAR)
-    ret.openpilotLongitudinalControl = experimental_long and ret.experimentalLongitudinalAvailable
-
-    ret.pcmCruise = not ret.openpilotLongitudinalControl
-
     # 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/car/tests/routes.py, we can remove it from this list.
     ret.dashcamOnly = candidate in {CAR.KIA_OPTIMA_H, CAR.ELANTRA_GT_I30}
 
+    if candidate in CANFD_CAR:
+      # detect HDA2 with LKAS message
+      if 0x50 in fingerprint[6]:
+        ret.flags |= HyundaiFlags.CANFD_HDA2.value
+      else:
+        # non-HDA2
+        if 0x1cf not in fingerprint[4]:
+          ret.flags |= HyundaiFlags.CANFD_ALT_BUTTONS.value
+
     ret.steerActuatorDelay = 0.1  # Default delay
     ret.steerLimitTimer = 0.4
     tire_stiffness_factor = 1.
 
-    ret.stoppingControl = True
-    ret.startingState = True
-    ret.vEgoStarting = 0.1
-    ret.startAccel = 2.0
-
-    ret.longitudinalTuning.kpV = [0.5]
-    ret.longitudinalTuning.kiV = [0.0]
-
-    ret.longitudinalActuatorDelayLowerBound = 0.5  # s
-    ret.longitudinalActuatorDelayUpperBound = 0.5  # s
     if candidate in (CAR.SANTA_FE, CAR.SANTA_FE_2022, CAR.SANTA_FE_HEV_2022, CAR.SANTA_FE_PHEV_2022):
       ret.lateralTuning.pid.kf = 0.00005
       ret.mass = 3982. * CV.LB_TO_KG + STD_CARGO_KG
@@ -202,12 +195,12 @@ class CarInterface(CarInterfaceBase):
       ret.lateralTuning.indi.timeConstantV = [1.4]
       ret.lateralTuning.indi.actuatorEffectivenessBP = [0.]
       ret.lateralTuning.indi.actuatorEffectivenessV = [1.8]
-    elif candidate in (CAR.KIA_OPTIMA, CAR.KIA_OPTIMA_2019, CAR.KIA_OPTIMA_H):
+    elif candidate in (CAR.KIA_OPTIMA_G4, CAR.KIA_OPTIMA_G4_FL, CAR.KIA_OPTIMA_H):
       ret.mass = 3558. * CV.LB_TO_KG
       ret.wheelbase = 2.80
       ret.steerRatio = 13.75
       tire_stiffness_factor = 0.5
-      if candidate == CAR.KIA_OPTIMA:
+      if candidate == CAR.KIA_OPTIMA_G4:
         ret.minSteerSpeed = 32 * CV.MPH_TO_MS
       CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
     elif candidate == CAR.KIA_STINGER:
@@ -291,19 +284,35 @@ class CarInterface(CarInterfaceBase):
       ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
       ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.16], [0.01]]
 
-    # panda safety config
+    # *** longitudinal control ***
+    if candidate in CANFD_CAR:
+      ret.longitudinalTuning.kpV = [0.1]
+      ret.longitudinalTuning.kiV = [0.0]
+      ret.longitudinalActuatorDelayLowerBound = 0.15
+      ret.longitudinalActuatorDelayUpperBound = 0.5
+      ret.experimentalLongitudinalAvailable = bool(ret.flags & HyundaiFlags.CANFD_HDA2)
+    else:
+      ret.longitudinalTuning.kpV = [0.5]
+      ret.longitudinalTuning.kiV = [0.0]
+      ret.longitudinalActuatorDelayLowerBound = 0.5
+      ret.longitudinalActuatorDelayUpperBound = 0.5
+      ret.experimentalLongitudinalAvailable = candidate not in (LEGACY_SAFETY_MODE_CAR | CAMERA_SCC_CAR)
+    ret.openpilotLongitudinalControl = experimental_long and ret.experimentalLongitudinalAvailable
+    ret.pcmCruise = not ret.openpilotLongitudinalControl
+
+    ret.stoppingControl = True
+    ret.startingState = True
+    ret.vEgoStarting = 0.1
+    ret.startAccel = 2.0
+
+    # *** panda safety config ***
     if candidate in CANFD_CAR:
       ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.noOutput),
                            get_safety_config(car.CarParams.SafetyModel.hyundaiCanfd)]
 
-      # detect HDA2 with LKAS message
+      if ret.flags & HyundaiFlags.CANFD_HDA2:
-      if 0x50 in fingerprint[6]:
-        ret.flags |= HyundaiFlags.CANFD_HDA2.value
         ret.safetyConfigs[1].safetyParam |= Panda.FLAG_HYUNDAI_CANFD_HDA2
-      else:
+      if ret.flags & HyundaiFlags.CANFD_ALT_BUTTONS:
-        # non-HDA2
-        if 0x1cf not in fingerprint[4]:
-          ret.flags |= HyundaiFlags.CANFD_ALT_BUTTONS.value
         ret.safetyConfigs[1].safetyParam |= Panda.FLAG_HYUNDAI_CANFD_ALT_BUTTONS
     else:
       ret.enableBsm = 0x58b in fingerprint[0]
@@ -314,18 +323,16 @@ class CarInterface(CarInterfaceBase):
       else:
         ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.hyundai, 0)]
 
-      # set appropriate safety param for gas signal
-      if candidate in HYBRID_CAR:
-        ret.safetyConfigs[0].safetyParam = 2
-      elif candidate in EV_CAR:
-        ret.safetyConfigs[0].safetyParam = 1
-
-      if ret.openpilotLongitudinalControl:
-        ret.safetyConfigs[0].safetyParam |= Panda.FLAG_HYUNDAI_LONG
-
       if candidate in CAMERA_SCC_CAR:
         ret.safetyConfigs[0].safetyParam |= Panda.FLAG_HYUNDAI_CAMERA_SCC
 
+    if ret.openpilotLongitudinalControl:
+      ret.safetyConfigs[-1].safetyParam |= Panda.FLAG_HYUNDAI_LONG
+    if candidate in HYBRID_CAR:
+      ret.safetyConfigs[-1].safetyParam |= Panda.FLAG_HYUNDAI_HYBRID_GAS
+    elif candidate in EV_CAR:
+      ret.safetyConfigs[-1].safetyParam |= Panda.FLAG_HYUNDAI_EV_GAS
+
     ret.centerToFront = ret.wheelbase * 0.4
 
     # TODO: get actual value, for now starting with reasonable value for
@@ -336,13 +343,15 @@ class CarInterface(CarInterfaceBase):
     # mass and CG position, so all cars will have approximately similar dyn behaviors
     ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront,
                                                                          tire_stiffness_factor=tire_stiffness_factor)
-
     return ret
 
   @staticmethod
   def init(CP, logcan, sendcan):
     if CP.openpilotLongitudinalControl:
-      disable_ecu(logcan, sendcan, addr=0x7d0, com_cont_req=b'\x28\x83\x01')
+      addr, bus = 0x7d0, 0
+      if CP.flags & HyundaiFlags.CANFD_HDA2.value:
+        addr, bus = 0x730, 5
+      disable_ecu(logcan, sendcan, bus=bus, addr=addr, com_cont_req=b'\x28\x83\x01')
 
   def _update(self, c):
     ret = self.CS.update(self.cp, self.cp_cam)

selfdrive/car/hyundai/values.py

@@ -85,8 +85,8 @@ class CAR:
   KIA_NIRO_EV = "KIA NIRO EV 2020"
   KIA_NIRO_PHEV = "KIA NIRO HYBRID 2019"
   KIA_NIRO_HEV_2021 = "KIA NIRO HYBRID 2021"
-  KIA_OPTIMA = "KIA OPTIMA 2016"
+  KIA_OPTIMA_G4 = "KIA OPTIMA 4TH GEN"
-  KIA_OPTIMA_2019 = "KIA OPTIMA 2019"
+  KIA_OPTIMA_G4_FL = "KIA OPTIMA 4TH GEN FACELIFT"
   KIA_OPTIMA_H = "KIA OPTIMA HYBRID 2017 & SPORTS 2019"
   KIA_SELTOS = "KIA SELTOS 2021"
   KIA_SORENTO = "KIA SORENTO GT LINE 2018"
@@ -109,11 +109,14 @@ class HyundaiCarInfo(CarInfo):
 CAR_INFO: Dict[str, Optional[Union[HyundaiCarInfo, List[HyundaiCarInfo]]]] = {
   CAR.ELANTRA: HyundaiCarInfo("Hyundai Elantra 2017-19", min_enable_speed=19 * CV.MPH_TO_MS, harness=Harness.hyundai_b),
   CAR.ELANTRA_2021: HyundaiCarInfo("Hyundai Elantra 2021-22", video_link="https://youtu.be/_EdYQtV52-c", harness=Harness.hyundai_k),
-  CAR.ELANTRA_HEV_2021: HyundaiCarInfo("Hyundai Elantra Hybrid 2021-22", video_link="https://youtu.be/_EdYQtV52-c", harness=Harness.hyundai_k),
+  CAR.ELANTRA_HEV_2021: HyundaiCarInfo("Hyundai Elantra Hybrid 2021-23", video_link="https://youtu.be/_EdYQtV52-c", harness=Harness.hyundai_k),
-  CAR.ELANTRA_GT_I30: None,  # dashcamOnly and same platform as CAR.ELANTRA
+  CAR.ELANTRA_GT_I30: [
+    HyundaiCarInfo("Hyundai Elantra GT 2017-19", harness=Harness.hyundai_e),
+    HyundaiCarInfo("Hyundai i30 2019", harness=Harness.hyundai_e),
+  ],
   CAR.HYUNDAI_GENESIS: HyundaiCarInfo("Hyundai Genesis 2015-16", min_enable_speed=19 * CV.MPH_TO_MS, harness=Harness.hyundai_j),  # TODO: check 2015 packages
   CAR.IONIQ: HyundaiCarInfo("Hyundai Ioniq Hybrid 2017-19", harness=Harness.hyundai_c),
-  CAR.IONIQ_HEV_2022: HyundaiCarInfo("Hyundai Ioniq Hybrid 2020-22", "Smart Cruise Control (SCC)", harness=Harness.hyundai_h),  # TODO: confirm 2020-21 harness
+  CAR.IONIQ_HEV_2022: HyundaiCarInfo("Hyundai Ioniq Hybrid 2020-22", harness=Harness.hyundai_h),  # TODO: confirm 2020-21 harness
   CAR.IONIQ_EV_LTD: HyundaiCarInfo("Hyundai Ioniq Electric 2019", harness=Harness.hyundai_c),
   CAR.IONIQ_EV_2020: HyundaiCarInfo("Hyundai Ioniq Electric 2020", "All", harness=Harness.hyundai_h),
   CAR.IONIQ_PHEV_2019: HyundaiCarInfo("Hyundai Ioniq Plug-in Hybrid 2019", harness=Harness.hyundai_c),
@@ -123,52 +126,58 @@ CAR_INFO: Dict[str, Optional[Union[HyundaiCarInfo, List[HyundaiCarInfo]]]] = {
   CAR.KONA_EV_2022: HyundaiCarInfo("Hyundai Kona Electric 2022", harness=Harness.hyundai_o),
   CAR.KONA_HEV: HyundaiCarInfo("Hyundai Kona Hybrid 2020", video_link="https://youtu.be/0dwpAHiZgFo", harness=Harness.hyundai_i),  # TODO: check packages
   CAR.SANTA_FE: HyundaiCarInfo("Hyundai Santa Fe 2019-20", "All", harness=Harness.hyundai_d),
-  CAR.SANTA_FE_2022: HyundaiCarInfo("Hyundai Santa Fe 2021-22", "All", video_link="https://youtu.be/VnHzSTygTS4", harness=Harness.hyundai_l),
+  CAR.SANTA_FE_2022: HyundaiCarInfo("Hyundai Santa Fe 2021-22", "All", "https://youtu.be/VnHzSTygTS4", harness=Harness.hyundai_l),
   CAR.SANTA_FE_HEV_2022: HyundaiCarInfo("Hyundai Santa Fe Hybrid 2022", "All", harness=Harness.hyundai_l),
   CAR.SANTA_FE_PHEV_2022: HyundaiCarInfo("Hyundai Santa Fe Plug-in Hybrid 2022", "All", harness=Harness.hyundai_l),
-  CAR.SONATA: HyundaiCarInfo("Hyundai Sonata 2020-22", "All", video_link="https://www.youtube.com/watch?v=ix63r9kE3Fw", harness=Harness.hyundai_a),
+  CAR.SONATA: HyundaiCarInfo("Hyundai Sonata 2020-22", "All", "https://www.youtube.com/watch?v=ix63r9kE3Fw", harness=Harness.hyundai_a),
   CAR.SONATA_LF: HyundaiCarInfo("Hyundai Sonata 2018-19", harness=Harness.hyundai_e),
   CAR.TUCSON: [
     HyundaiCarInfo("Hyundai Tucson 2021", min_enable_speed=19 * CV.MPH_TO_MS, harness=Harness.hyundai_l),
     HyundaiCarInfo("Hyundai Tucson Diesel 2019", harness=Harness.hyundai_l),
   ],
   CAR.PALISADE: [
-    HyundaiCarInfo("Hyundai Palisade 2020-22", "All", video_link="https://youtu.be/TAnDqjF4fDY?t=456", harness=Harness.hyundai_h),
+    HyundaiCarInfo("Hyundai Palisade 2020-22", "All", "https://youtu.be/TAnDqjF4fDY?t=456", harness=Harness.hyundai_h),
     HyundaiCarInfo("Kia Telluride 2020", "All", harness=Harness.hyundai_h),
   ],
   CAR.VELOSTER: HyundaiCarInfo("Hyundai Veloster 2019-20", min_enable_speed=5. * CV.MPH_TO_MS, harness=Harness.hyundai_e),
   CAR.SONATA_HYBRID: HyundaiCarInfo("Hyundai Sonata Hybrid 2020-22", "All", harness=Harness.hyundai_a),
-  CAR.IONIQ_5: HyundaiCarInfo("Hyundai Ioniq 5 2022", "Highway Driving Assist II", harness=Harness.hyundai_q),
+  CAR.IONIQ_5: [
+    HyundaiCarInfo("Hyundai Ioniq 5 (without HDA II) 2022" , "Highway Driving Assist", harness=Harness.hyundai_k),
+    HyundaiCarInfo("Hyundai Ioniq 5 (with HDA II) 2022", "Highway Driving Assist II", harness=Harness.hyundai_q),
+  ],
   CAR.TUCSON_HYBRID_4TH_GEN: HyundaiCarInfo("Hyundai Tucson Hybrid 2022", "All", harness=Harness.hyundai_n),
 
   # Kia
   CAR.KIA_FORTE: HyundaiCarInfo("Kia Forte 2019-21", harness=Harness.hyundai_g),
   CAR.KIA_K5_2021: HyundaiCarInfo("Kia K5 2021-22", harness=Harness.hyundai_a),
   CAR.KIA_NIRO_EV: [
-    HyundaiCarInfo("Kia Niro EV 2019", "All", video_link="https://www.youtube.com/watch?v=lT7zcG6ZpGo", harness=Harness.hyundai_h),
+    HyundaiCarInfo("Kia Niro EV 2019", "All", "https://www.youtube.com/watch?v=lT7zcG6ZpGo", harness=Harness.hyundai_h),
-    HyundaiCarInfo("Kia Niro EV 2020", "All", video_link="https://www.youtube.com/watch?v=lT7zcG6ZpGo", harness=Harness.hyundai_f),
+    HyundaiCarInfo("Kia Niro EV 2020", "All", "https://www.youtube.com/watch?v=lT7zcG6ZpGo", harness=Harness.hyundai_f),
-    HyundaiCarInfo("Kia Niro EV 2021", "All", video_link="https://www.youtube.com/watch?v=lT7zcG6ZpGo", harness=Harness.hyundai_c),
+    HyundaiCarInfo("Kia Niro EV 2021", "All", "https://www.youtube.com/watch?v=lT7zcG6ZpGo", harness=Harness.hyundai_c),
-    HyundaiCarInfo("Kia Niro EV 2022", "All", video_link="https://www.youtube.com/watch?v=lT7zcG6ZpGo", harness=Harness.hyundai_h),
+    HyundaiCarInfo("Kia Niro EV 2022", "All", "https://www.youtube.com/watch?v=lT7zcG6ZpGo", harness=Harness.hyundai_h),
   ],
   CAR.KIA_NIRO_PHEV: HyundaiCarInfo("Kia Niro Plug-in Hybrid 2018-19", "All", min_enable_speed=10. * CV.MPH_TO_MS, harness=Harness.hyundai_c),
   CAR.KIA_NIRO_HEV_2021: [
     HyundaiCarInfo("Kia Niro Hybrid 2021", harness=Harness.hyundai_f),  # TODO: could be hyundai_d, verify
     HyundaiCarInfo("Kia Niro Hybrid 2022", harness=Harness.hyundai_h),
   ],
-  CAR.KIA_OPTIMA: HyundaiCarInfo("Kia Optima 2017", "Advanced Smart Cruise Control", harness=Harness.hyundai_b),  # TODO: may support 2016, 2018
+  CAR.KIA_OPTIMA_G4: HyundaiCarInfo("Kia Optima 2017", "Advanced Smart Cruise Control", harness=Harness.hyundai_b),  # TODO: may support 2016, 2018
-  CAR.KIA_OPTIMA_2019: HyundaiCarInfo("Kia Optima 2019-20", harness=Harness.hyundai_g),
+  CAR.KIA_OPTIMA_G4_FL: HyundaiCarInfo("Kia Optima 2019-20", harness=Harness.hyundai_g),
   CAR.KIA_OPTIMA_H: [
     HyundaiCarInfo("Kia Optima Hybrid 2017", "Advanced Smart Cruise Control"),  # TODO: may support adjacent years
     HyundaiCarInfo("Kia Optima Hybrid 2019"),
   ],
   CAR.KIA_SELTOS: HyundaiCarInfo("Kia Seltos 2021", harness=Harness.hyundai_a),
   CAR.KIA_SORENTO: [
-    HyundaiCarInfo("Kia Sorento 2018", "Advanced Smart Cruise Control", video_link="https://www.youtube.com/watch?v=Fkh3s6WHJz8", harness=Harness.hyundai_c),
+    HyundaiCarInfo("Kia Sorento 2018", "Advanced Smart Cruise Control", "https://www.youtube.com/watch?v=Fkh3s6WHJz8", harness=Harness.hyundai_c),
     HyundaiCarInfo("Kia Sorento 2019", video_link="https://www.youtube.com/watch?v=Fkh3s6WHJz8", harness=Harness.hyundai_e),
   ],
   CAR.KIA_STINGER: HyundaiCarInfo("Kia Stinger 2018-20", video_link="https://www.youtube.com/watch?v=MJ94qoofYw0", harness=Harness.hyundai_c),
   CAR.KIA_CEED: HyundaiCarInfo("Kia Ceed 2019", harness=Harness.hyundai_e),
-  CAR.KIA_EV6: HyundaiCarInfo("Kia EV6 2022", "Highway Driving Assist II", harness=Harness.hyundai_p),
+  CAR.KIA_EV6: [
+    HyundaiCarInfo("Kia EV6 (without HDA II) 2022", "Highway Driving Assist", harness=Harness.hyundai_l),
+    HyundaiCarInfo("Kia EV6 (with HDA II) 2022", "Highway Driving Assist II", harness=Harness.hyundai_p)
+  ],
 
   # Genesis
   CAR.GENESIS_G70: HyundaiCarInfo("Genesis G70 2018-19", "All", harness=Harness.hyundai_f),
@@ -290,6 +299,9 @@ FW_QUERY_CONFIG = FwQueryConfig(
       [HYUNDAI_VERSION_RESPONSE],
     ),
   ],
+  extra_ecus=[
+    (Ecu.adas, 0x730, None),  # ADAS Driving ECU on HDA2 platforms
+  ],
 )
 
 FW_VERSIONS = {
@@ -1136,7 +1148,7 @@ FW_VERSIONS = {
       b'\xf1\x87954A22D200\xf1\x81T01950A1  \xf1\000T0190XBL  T01950A1  DSP2T16X4X950NS8\r\xfe\x9c\x8b',
     ],
   },
-  CAR.KIA_OPTIMA: {
+  CAR.KIA_OPTIMA_G4: {
     (Ecu.fwdRadar, 0x7d0, None): [
       b'\xf1\x00JF__ SCC F-CUP      1.00 1.00 96400-D4100         ',
     ],
@@ -1150,7 +1162,7 @@ FW_VERSIONS = {
       b'\xf1\x87\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xf1\x816T6J0051\x00\x00\xf1\x006T6J0_C2\x00\x006T6J0051\x00\x00TJF0T20NSB\x00\x00\x00\x00',
     ],
   },
-  CAR.KIA_OPTIMA_2019: {
+  CAR.KIA_OPTIMA_G4_FL: {
     (Ecu.fwdRadar, 0x7d0, None): [
       b'\xf1\x00JF__ SCC F-CUP      1.00 1.00 96400-D4110         ',
     ],
@@ -1218,12 +1230,14 @@ FW_VERSIONS = {
     (Ecu.fwdCamera, 0x7c4, None): [
       b'\xf1\x00CN7HMFC  AT USA LHD 1.00 1.05 99210-AA000 210930',
       b'\xf1\000CN7HMFC  AT USA LHD 1.00 1.03 99210-AA000 200819',
+      b'\xf1\x00CN7HMFC  AT USA LHD 1.00 1.07 99210-AA000 220426',
     ],
     (Ecu.fwdRadar, 0x7d0, None): [
       b'\xf1\000CNhe SCC FHCUP      1.00 1.01 99110-BY000         ',
       b'\xf1\x8799110BY000\xf1\x00CNhe SCC FHCUP      1.00 1.01 99110-BY000         ',
     ],
     (Ecu.eps, 0x7d4, None): [
+      b'\xf1\x00CN7 MDPS C 1.00 1.03 56310BY0500 4CNHC103',
       b'\xf1\x8756310/BY050\xf1\x00CN7 MDPS C 1.00 1.03 56310/BY050 4CNHC103',
       b'\xf1\x8756310/BY050\xf1\000CN7 MDPS C 1.00 1.02 56310/BY050 4CNHC102',
     ],
@@ -1235,6 +1249,7 @@ FW_VERSIONS = {
     ],
     (Ecu.engine, 0x7e0, None): [
       b'\xf1\x816H6G5051\x00\x00\x00\x00\x00\x00\x00\x00',
+      b'\xf1\x816H6G6051\x00\x00\x00\x00\x00\x00\x00\x00',
     ]
   },
   CAR.KONA_HEV: {
@@ -1307,11 +1322,13 @@ FW_VERSIONS = {
   },
   CAR.KIA_EV6: {
     (Ecu.abs, 0x7d1, None): [
+      b'\xf1\x00CV  IEB \x02 101!\x10\x18 58520-CV100',
       b'\xf1\x00CV  IEB \x03 101!\x10\x18 58520-CV100',
       b'\xf1\x8758520CV100\xf1\x00CV  IEB \x02 101!\x10\x18 58520-CV100',
     ],
     (Ecu.eps, 0x7d4, None): [
       b'\xf1\x00CV1 MDPS R 1.00 1.04 57700-CV000 1B30',
+      b'\xf1\x00CV1 MDPS R 1.00 1.05 57700-CV000 2425',
     ],
     (Ecu.fwdRadar, 0x7d0, None): [
       b'\xf1\x00CV1_ RDR -----      1.00 1.01 99110-CV000         ',
@@ -1319,6 +1336,7 @@ FW_VERSIONS = {
     ],
     (Ecu.fwdCamera, 0x7c4, None): [
       b'\xf1\x00CV1 MFC  AT USA LHD 1.00 1.05 99210-CV000 211027',
+      b'\xf1\x00CV1 MFC  AT USA LHD 1.00 1.06 99210-CV000 220328',
       b'\xf1\x00CV1 MFC  AT EUR LHD 1.00 1.05 99210-CV000 211027',
     ],
   },
@@ -1367,7 +1385,7 @@ CHECKSUM = {
 FEATURES = {
   # which message has the gear
   "use_cluster_gears": {CAR.ELANTRA, CAR.ELANTRA_GT_I30, CAR.KONA},
-  "use_tcu_gears": {CAR.KIA_OPTIMA, CAR.KIA_OPTIMA_2019, CAR.SONATA_LF, CAR.VELOSTER, CAR.TUCSON},
+  "use_tcu_gears": {CAR.KIA_OPTIMA_G4, CAR.KIA_OPTIMA_G4_FL, CAR.SONATA_LF, CAR.VELOSTER, CAR.TUCSON},
   "use_elect_gears": {CAR.KIA_NIRO_EV, CAR.KIA_NIRO_PHEV, CAR.KIA_NIRO_HEV_2021, CAR.KIA_OPTIMA_H, CAR.IONIQ_EV_LTD, CAR.KONA_EV, CAR.IONIQ, CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV, CAR.ELANTRA_HEV_2021, CAR.SONATA_HYBRID, CAR.KONA_HEV, CAR.IONIQ_HEV_2022, CAR.SANTA_FE_HEV_2022, CAR.SANTA_FE_PHEV_2022, CAR.IONIQ_PHEV_2019, CAR.KONA_EV_2022},
 
   # these cars use the FCA11 message for the AEB and FCW signals, all others use SCC12
@@ -1379,11 +1397,11 @@ CANFD_CAR = {CAR.KIA_EV6, CAR.IONIQ_5, CAR.TUCSON_HYBRID_4TH_GEN}
 # The camera does SCC on these cars, rather than the radar
 CAMERA_SCC_CAR = {CAR.KONA_EV_2022, }
 
-HYBRID_CAR = {CAR.IONIQ_PHEV, CAR.ELANTRA_HEV_2021, CAR.KIA_NIRO_PHEV, CAR.KIA_NIRO_HEV_2021, CAR.SONATA_HYBRID, CAR.KONA_HEV, CAR.IONIQ, CAR.IONIQ_HEV_2022, CAR.SANTA_FE_HEV_2022, CAR.SANTA_FE_PHEV_2022, CAR.IONIQ_PHEV_2019}  # these cars use a different gas signal
+HYBRID_CAR = {CAR.IONIQ_PHEV, CAR.ELANTRA_HEV_2021, CAR.KIA_NIRO_PHEV, CAR.KIA_NIRO_HEV_2021, CAR.SONATA_HYBRID, CAR.KONA_HEV, CAR.IONIQ, CAR.IONIQ_HEV_2022, CAR.SANTA_FE_HEV_2022, CAR.SANTA_FE_PHEV_2022, CAR.IONIQ_PHEV_2019, CAR.TUCSON_HYBRID_4TH_GEN}  # these cars use a different gas signal
-EV_CAR = {CAR.IONIQ_EV_2020, CAR.IONIQ_EV_LTD, CAR.KONA_EV, CAR.KIA_NIRO_EV, CAR.KONA_EV_2022}
+EV_CAR = {CAR.IONIQ_EV_2020, CAR.IONIQ_EV_LTD, CAR.KONA_EV, CAR.KIA_NIRO_EV, CAR.KONA_EV_2022, CAR.KIA_EV6, CAR.IONIQ_5}
 
 # these cars require a special panda safety mode due to missing counters and checksums in the messages
-LEGACY_SAFETY_MODE_CAR = {CAR.HYUNDAI_GENESIS, CAR.IONIQ_EV_2020, CAR.IONIQ_EV_LTD, CAR.IONIQ_PHEV, CAR.IONIQ, CAR.KONA_EV, CAR.KIA_SORENTO, CAR.SONATA_LF, CAR.KIA_OPTIMA, CAR.KIA_OPTIMA_2019, CAR.VELOSTER, CAR.KIA_STINGER, CAR.GENESIS_G70, CAR.GENESIS_G80, CAR.KIA_CEED, CAR.ELANTRA, CAR.IONIQ_HEV_2022}
+LEGACY_SAFETY_MODE_CAR = {CAR.HYUNDAI_GENESIS, CAR.IONIQ_EV_2020, CAR.IONIQ_EV_LTD, CAR.IONIQ_PHEV, CAR.IONIQ, CAR.KONA_EV, CAR.KIA_SORENTO, CAR.SONATA_LF, CAR.KIA_OPTIMA_G4, CAR.KIA_OPTIMA_G4_FL, CAR.VELOSTER, CAR.KIA_STINGER, CAR.GENESIS_G70, CAR.GENESIS_G80, CAR.KIA_CEED, CAR.ELANTRA, CAR.IONIQ_HEV_2022}
 
 # If 0x500 is present on bus 1 it probably has a Mando radar outputting radar points.
 # If no points are outputted by default it might be possible to turn it on using  selfdrive/debug/hyundai_enable_radar_points.py
@@ -1408,8 +1426,8 @@ DBC = {
   CAR.KIA_NIRO_EV: dbc_dict('hyundai_kia_generic', 'hyundai_kia_mando_front_radar_generated'),
   CAR.KIA_NIRO_PHEV: dbc_dict('hyundai_kia_generic', 'hyundai_kia_mando_front_radar_generated'),
   CAR.KIA_NIRO_HEV_2021: dbc_dict('hyundai_kia_generic', None),
-  CAR.KIA_OPTIMA: dbc_dict('hyundai_kia_generic', None),
+  CAR.KIA_OPTIMA_G4: dbc_dict('hyundai_kia_generic', None),
-  CAR.KIA_OPTIMA_2019: dbc_dict('hyundai_kia_generic', None),
+  CAR.KIA_OPTIMA_G4_FL: dbc_dict('hyundai_kia_generic', None),
   CAR.KIA_OPTIMA_H: dbc_dict('hyundai_kia_generic', None),
   CAR.KIA_SELTOS: dbc_dict('hyundai_kia_generic', None),
   CAR.KIA_SORENTO: dbc_dict('hyundai_kia_generic', None), # Has 0x5XX messages, but different format

selfdrive/car/interfaces.py

@@ -10,11 +10,12 @@ from common.conversions import Conversions as CV
 from common.kalman.simple_kalman import KF1D
 from common.numpy_fast import interp
 from common.realtime import DT_CTRL
-from selfdrive.car import apply_hysteresis, create_button_enable_events, gen_empty_fingerprint
+from selfdrive.car import apply_hysteresis, gen_empty_fingerprint
 from selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX, apply_deadzone
 from selfdrive.controls.lib.events import Events
 from selfdrive.controls.lib.vehicle_model import VehicleModel
 
+ButtonType = car.CarState.ButtonEvent.Type
 GearShifter = car.CarState.GearShifter
 EventName = car.CarEvent.EventName
 TorqueFromLateralAccelCallbackType = Callable[[float, car.CarParams.LateralTorqueTuning, float, float, bool], float]
@@ -210,7 +211,8 @@ class CarInterfaceBase(ABC):
   def apply(self, c: car.CarControl) -> Tuple[car.CarControl.Actuators, List[bytes]]:
     pass
 
-  def create_common_events(self, cs_out, extra_gears=None, pcm_enable=True, allow_enable=True):
+  def create_common_events(self, cs_out, extra_gears=None, pcm_enable=True, allow_enable=True,
+                           enable_buttons=(ButtonType.accelCruise, ButtonType.decelCruise)):
     events = Events()
 
     if cs_out.doorOpen:
@@ -244,7 +246,13 @@ class CarInterfaceBase(ABC):
       events.add(EventName.steerOverride)
 
     # Handle button presses
-    events.events.extend(create_button_enable_events(cs_out.buttonEvents, pcm_cruise=self.CP.pcmCruise))
+    for b in cs_out.buttonEvents:
+      # Enable OP long on falling edge of enable buttons (defaults to accelCruise and decelCruise, overridable per-port)
+      if not self.CP.pcmCruise and (b.type in enable_buttons and not b.pressed):
+        events.add(EventName.buttonEnable)
+      # Disable on rising edge of cancel for both stock and OP long
+      if b.type == ButtonType.cancel and b.pressed:
+        events.add(EventName.buttonCancel)
 
     # Handle permanent and temporary steering faults
     self.steering_unpressed = 0 if cs_out.steeringPressed else self.steering_unpressed + 1

selfdrive/car/isotp_parallel_query.py

@@ -1,7 +1,6 @@
 import time
 from collections import defaultdict
 from functools import partial
-from typing import Optional
 
 import cereal.messaging as messaging
 from system.swaglog import cloudlog
@@ -10,24 +9,21 @@ from panda.python.uds import CanClient, IsoTpMessage, FUNCTIONAL_ADDRS, get_rx_a
 
 
 class IsoTpParallelQuery:
-  def __init__(self, sendcan, logcan, bus, addrs, request, response, response_offset=0x8, functional_addr=False, debug=False, response_pending_timeout=10):
+  def __init__(self, sendcan, logcan, bus, addrs, request, response, response_offset=0x8, functional_addrs=None, debug=False, response_pending_timeout=10):
     self.sendcan = sendcan
     self.logcan = logcan
     self.bus = bus
     self.request = request
     self.response = response
+    self.functional_addrs = functional_addrs or []
     self.debug = debug
-    self.functional_addr = functional_addr
     self.response_pending_timeout = response_pending_timeout
 
-    self.real_addrs = []
+    real_addrs = [a if isinstance(a, tuple) else (a, None) for a in addrs]
-    for a in addrs:
+    for tx_addr, _ in real_addrs:
-      if isinstance(a, tuple):
+      assert tx_addr not in FUNCTIONAL_ADDRS, f"Functional address should be defined in functional_addrs: {hex(tx_addr)}"
-        self.real_addrs.append(a)
-      else:
-        self.real_addrs.append((a, None))
 
-    self.msg_addrs = {tx_addr: get_rx_addr_for_tx_addr(tx_addr[0], rx_offset=response_offset) for tx_addr in self.real_addrs}
+    self.msg_addrs = {tx_addr: get_rx_addr_for_tx_addr(tx_addr[0], rx_offset=response_offset) for tx_addr in real_addrs}
     self.msg_buffer = defaultdict(list)
 
   def rx(self):
@@ -36,12 +32,7 @@ class IsoTpParallelQuery:
 
     for packet in can_packets:
       for msg in packet.can:
-        if msg.src == self.bus:
+        if msg.src == self.bus and msg.address in self.msg_addrs.values():
-          if self.functional_addr:
-            if (0x7E8 <= msg.address <= 0x7EF) or (0x18DAF100 <= msg.address <= 0x18DAF1FF):
-              fn_addr = next(a for a in FUNCTIONAL_ADDRS if msg.address - a <= 32)
-              self.msg_buffer[fn_addr].append((msg.address, msg.busTime, msg.dat, msg.src))
-          elif msg.address in self.msg_addrs.values():
           self.msg_buffer[msg.address].append((msg.address, msg.busTime, msg.dat, msg.src))
 
   def _can_tx(self, tx_addr, dat, bus):
@@ -72,6 +63,16 @@ class IsoTpParallelQuery:
     messaging.drain_sock(self.logcan)
     self.msg_buffer = defaultdict(list)
 
+  def _create_isotp_msg(self, tx_addr, sub_addr, rx_addr):
+    can_client = CanClient(self._can_tx, partial(self._can_rx, rx_addr, sub_addr=sub_addr), tx_addr, rx_addr,
+                           self.bus, sub_addr=sub_addr, debug=self.debug)
+
+    max_len = 8 if sub_addr is None else 7
+    # uses iso-tp frame separation time of 10 ms
+    # TODO: use single_frame_mode so ECUs can send as fast as they want,
+    # as well as reduces chances we process messages from previous queries
+    return IsoTpMessage(can_client, timeout=0, separation_time=0.01, debug=self.debug, max_len=max_len)
+
   def get_data(self, timeout, total_timeout=60.):
     self._drain_rx()
 
@@ -80,22 +81,19 @@ class IsoTpParallelQuery:
     request_counter = {}
     request_done = {}
     for tx_addr, rx_addr in self.msg_addrs.items():
-      # rx_addr not set when using functional tx addr
+      msgs[tx_addr] = self._create_isotp_msg(*tx_addr, rx_addr)
-      id_addr = rx_addr or tx_addr[0]
-      sub_addr = tx_addr[1]
-
-      can_client = CanClient(self._can_tx, partial(self._can_rx, id_addr, sub_addr=sub_addr), tx_addr[0], rx_addr,
-                             self.bus, sub_addr=sub_addr, debug=self.debug)
-
-      max_len = 8 if sub_addr is None else 7
-
-      msg = IsoTpMessage(can_client, timeout=0, max_len=max_len, debug=self.debug)
-      msg.send(self.request[0])
-
-      msgs[tx_addr] = msg
       request_counter[tx_addr] = 0
       request_done[tx_addr] = False
 
+    # Send first request to functional addrs, subsequent responses are handled on physical addrs
+    if len(self.functional_addrs):
+      for addr in self.functional_addrs:
+        self._create_isotp_msg(addr, None, -1).send(self.request[0])
+
+    # If querying functional addrs, set up physical IsoTpMessages to send consecutive frames
+    for msg in msgs.values():
+      msg.send(self.request[0], setup_only=len(self.functional_addrs) > 0)
+
     results = {}
     start_time = time.monotonic()
     response_timeouts = {tx_addr: start_time + timeout for tx_addr in self.msg_addrs}
@@ -107,12 +105,15 @@ class IsoTpParallelQuery:
 
       for tx_addr, msg in msgs.items():
         try:
-          dat: Optional[bytes] = msg.recv()
+          dat, updated = msg.recv()
         except Exception:
-          cloudlog.exception("Error processing UDS response")
+          cloudlog.exception(f"Error processing UDS response: {tx_addr}")
           request_done[tx_addr] = True
           continue
 
+        if updated:
+          response_timeouts[tx_addr] = time.monotonic() + timeout
+
         if not dat:
           continue
 
@@ -121,12 +122,11 @@ class IsoTpParallelQuery:
         response_valid = dat[:len(expected_response)] == expected_response
 
         if response_valid:
-          response_timeouts[tx_addr] = time.monotonic() + timeout
           if counter + 1 < len(self.request):
             msg.send(self.request[counter + 1])
             request_counter[tx_addr] += 1
           else:
-            results[(tx_addr, msg._can_client.rx_addr)] = dat[len(expected_response):]
+            results[tx_addr] = dat[len(expected_response):]
             request_done[tx_addr] = True
         else:
           error_code = dat[2] if len(dat) > 2 else -1

selfdrive/car/mock/interface.py

@@ -20,8 +20,7 @@ class CarInterface(CarInterfaceBase):
 
     cloudlog.debug("Using Mock Car Interface")
 
-    self.sensor = messaging.sub_sock('sensorEvents')
+    self.sm = messaging.SubMaster(['gyroscope', 'gpsLocation', 'gpsLocationExternal'])
-    self.gps = messaging.sub_sock('gpsLocationExternal')
 
     self.speed = 0.
     self.prev_speed = 0.
@@ -45,17 +44,16 @@ class CarInterface(CarInterfaceBase):
 
   # returns a car.CarState
   def _update(self, c):
+    self.sm.update(0)
+
     # get basic data from phone and gps since CAN isn't connected
-    sensors = messaging.recv_sock(self.sensor)
+    if self.sm.updated['gyroscope']:
-    if sensors is not None:
+      self.yaw_rate_meas = -self.sm['gyroscope'].gyroUncalibrated.v[0]
-      for sensor in sensors.sensorEvents:
+
-        if sensor.type == 4:  # gyro
+    gps_sock = 'gpsLocationExternal' if self.sm.rcv_frame['gpsLocationExternal'] > 1 else 'gpsLocation'
-          self.yaw_rate_meas = -sensor.gyro.v[0]
+    if self.sm.updated[gps_sock]:
-
-    gps = messaging.recv_sock(self.gps)
-    if gps is not None:
       self.prev_speed = self.speed
-      self.speed = gps.gpsLocationExternal.speed
+      self.speed = self.sm[gps_sock].speed
 
     # create message
     ret = car.CarState.new_message()

selfdrive/car/subaru/carstate.py

@@ -58,9 +58,6 @@ class CarState(CarStateBase):
     ret.cruiseState.available = cp_cruise.vl["CruiseControl"]["Cruise_On"] != 0
     ret.cruiseState.speed = cp_cam.vl["ES_DashStatus"]["Cruise_Set_Speed"] * CV.KPH_TO_MS
 
-    if self.car_fingerprint not in PREGLOBAL_CARS:
-      ret.cruiseState.standstill = cp_cam.vl["ES_DashStatus"]["Cruise_State"] == 3
-
     if (self.car_fingerprint in PREGLOBAL_CARS and cp.vl["Dash_State2"]["UNITS"] == 1) or \
        (self.car_fingerprint not in PREGLOBAL_CARS and cp.vl["Dashlights"]["UNITS"] == 1):
       ret.cruiseState.speed *= CV.MPH_TO_KPH
@@ -78,6 +75,8 @@ class CarState(CarStateBase):
     else:
       ret.steerFaultTemporary = cp.vl["Steering_Torque"]["Steer_Warning"] == 1
       ret.cruiseState.nonAdaptive = cp_cam.vl["ES_DashStatus"]["Conventional_Cruise"] == 1
+      ret.cruiseState.standstill = cp_cam.vl["ES_DashStatus"]["Cruise_State"] == 3
+      ret.stockFcw = cp_cam.vl["ES_LKAS_State"]["LKAS_Alert"] == 2
       self.es_lkas_msg = copy.copy(cp_cam.vl["ES_LKAS_State"])
 
     cp_es_distance = cp_body if self.car_fingerprint in GLOBAL_GEN2 else cp_cam

selfdrive/car/subaru/values.py

@@ -124,23 +124,26 @@ FW_VERSIONS = {
   CAR.LEGACY: {
     (Ecu.abs, 0x7b0, None): [
       b'\xa1\\  x04\x01',
-      b'\xa1  \x03\x03'
+      b'\xa1  \x03\x03',
+      b'\xa1  \x02\x01',
     ],
     (Ecu.eps, 0x746, None): [
       b'\x9b\xc0\x11\x00',
-      b'\x9b\xc0\x11\x02'
+      b'\x9b\xc0\x11\x02',
     ],
     (Ecu.fwdCamera, 0x787, None): [
       b'\x00\x00e\x80\x00\x1f@ \x19\x00',
-      b'\x00\x00e\x9a\x00\x00\x00\x00\x00\x00'
+      b'\x00\x00e\x9a\x00\x00\x00\x00\x00\x00',
     ],
     (Ecu.engine, 0x7e0, None): [
       b'\xde\"a0\x07',
-      b'\xe2"aq\x07'
+      b'\xe2"aq\x07',
+      b'\xde,\xa0@\x07',
     ],
     (Ecu.transmission, 0x7e1, None): [
       b'\xa5\xf6\x05@\x00',
-      b'\xa7\xf6\x04@\x00'
+      b'\xa7\xf6\x04@\x00',
+      b'\xa5\xfe\xc7@\x00',
     ],
   },
   CAR.IMPREZA: {

selfdrive/car/tesla/values.py

@@ -1,9 +1,12 @@
 from collections import namedtuple
 from typing import Dict, List, Union
 
+from cereal import car
 from selfdrive.car import dbc_dict
 from selfdrive.car.docs_definitions import CarInfo
-from cereal import car
+from selfdrive.car.fw_query_definitions import FwQueryConfig, Request, StdQueries
+
+Ecu = car.CarParams.Ecu
 
 Button = namedtuple('Button', ['event_type', 'can_addr', 'can_msg', 'values'])
 AngleRateLimit = namedtuple('AngleRateLimit', ['speed_points', 'max_angle_diff_points'])
@@ -20,11 +23,6 @@ CAR_INFO: Dict[str, Union[CarInfo, List[CarInfo]]] = {
 }
 
 FINGERPRINTS = {
-  CAR.AP2_MODELS: [
-    {
-      1: 8, 3: 8, 14: 8, 21: 4, 69: 8, 109: 4, 257: 3, 264: 8, 277: 6, 280: 6, 293: 4, 296: 4, 309: 5, 325: 8, 328: 5, 336: 8, 341: 8, 360: 7, 373: 8, 389: 8, 415: 8, 513: 5, 516: 8, 518: 8, 520: 4, 522: 8, 524: 8, 526: 8, 532: 3, 536: 8, 537: 3, 538: 8, 542: 8, 551: 5, 552: 2, 556: 8, 558: 8, 568: 8, 569: 8, 574: 8, 576: 3, 577: 8, 582: 5, 583: 8, 584: 4, 585: 8, 590: 8, 601: 8, 606: 8, 608: 1, 622: 8, 627: 6, 638: 8, 641: 8, 643: 8, 692: 8, 693: 8, 695: 8, 696: 8, 697: 8, 699: 8, 700: 8, 701: 8, 702: 8, 703: 8, 704: 8, 708: 8, 709: 8, 710: 8, 711: 8, 712: 8, 728: 8, 744: 8, 760: 8, 772: 8, 775: 8, 776: 8, 777: 8, 778: 8, 782: 8, 788: 8, 791: 8, 792: 8, 796: 2, 797: 8, 798: 6, 799: 8, 804: 8, 805: 8, 807: 8, 808: 1, 811: 8, 812: 8, 813: 8, 814: 5, 815: 8, 820: 8, 823: 8, 824: 8, 829: 8, 830: 5, 836: 8, 840: 8, 845: 8, 846: 5, 848: 8, 852: 8, 853: 8, 856: 4, 857: 6, 861: 8, 862: 5, 872: 8, 876: 8, 877: 8, 879: 8, 880: 8, 882: 8, 884: 8, 888: 8, 893: 8, 894: 8, 901: 6, 904: 3, 905: 8, 906: 8, 908: 2, 909: 8, 910: 8, 912: 8, 920: 8, 921: 8, 925: 4, 926: 6, 936: 8, 941: 8, 949: 8, 952: 8, 953: 6, 968: 8, 969: 7, 970: 8, 971: 8, 977: 8, 984: 8, 986: 8, 987: 8, 990: 8, 1000: 8, 1001: 8, 1006: 8, 1007: 8, 1008: 8, 1010: 6, 1014: 1, 1015: 8, 1016: 8, 1017: 8, 1018: 8, 1020: 8, 1026: 8, 1028: 8, 1029: 8, 1030: 8, 1032: 1, 1033: 1, 1034: 8, 1048: 1, 1049: 8, 1061: 8, 1064: 8, 1065: 8, 1070: 8, 1080: 8, 1081: 8, 1097: 8, 1113: 8, 1129: 8, 1145: 8, 1160: 4, 1177: 8, 1281: 8, 1328: 8, 1329: 8, 1332: 8, 1335: 8, 1337: 8, 1353: 8, 1368: 8, 1412: 8, 1436: 8, 1476: 8, 1481: 8, 1497: 8, 1513: 8, 1519: 8, 1601: 8, 1605: 8, 1617: 8, 1621: 8, 1625: 8, 1665: 8, 1792: 8, 1798: 8, 1800: 4, 1804: 8, 1812: 8, 1815: 8, 1816: 8, 1824: 8, 1825: 8, 1828: 8, 1831: 8, 1832: 8, 1840: 8, 1842: 8, 1848: 8, 1864: 8, 1872: 8, 1880: 8, 1888: 8, 1892: 8, 1896: 8, 1912: 8, 1937: 8, 1953: 8, 1960: 8, 1968: 8, 1992: 8, 2001: 8, 2008: 3, 2015: 8, 2016: 8, 2043: 5, 2045: 4
-    },
-  ],
   CAR.AP1_MODELS: [
     {
       1: 8, 3: 8, 14: 8, 21: 4, 69: 8, 109: 4, 257: 3, 264: 8, 267: 5, 277: 6, 280: 6, 283: 5, 293: 4, 296: 4, 309: 5, 325: 8, 328: 5, 336: 8, 341: 8, 360: 7, 373: 8, 389: 8, 415: 8, 513: 5, 516: 8, 520: 4, 522: 8, 524: 8, 526: 8, 532: 3, 536: 8, 537: 3, 542: 8, 551: 5, 552: 2, 556: 8, 558: 8, 568: 8, 569: 8, 574: 8, 577: 8, 582: 5, 584: 4, 585: 8, 590: 8, 606: 8, 622: 8, 627: 6, 638: 8, 641: 8, 643: 8, 660: 5, 693: 8, 696: 8, 697: 8, 712: 8, 728: 8, 744: 8, 760: 8, 772: 8, 775: 8, 776: 8, 777: 8, 778: 8, 782: 8, 788: 8, 791: 8, 792: 8, 796: 2, 797: 8, 798: 6, 799: 8, 804: 8, 805: 8, 807: 8, 808: 1, 809: 8, 812: 8, 813: 8, 814: 5, 815: 8, 820: 8, 823: 8, 824: 8, 829: 8, 830: 5, 836: 8, 840: 8, 841: 8, 845: 8, 846: 5, 852: 8, 856: 4, 857: 6, 861: 8, 862: 5, 872: 8, 873: 8, 877: 8, 878: 8, 879: 8, 880: 8, 884: 8, 888: 8, 889: 8, 893: 8, 896: 8, 901: 6, 904: 3, 905: 8, 908: 2, 909: 8, 920: 8, 921: 8, 925: 4, 936: 8, 937: 8, 941: 8, 949: 8, 952: 8, 953: 6, 957: 8, 968: 8, 973: 8, 984: 8, 987: 8, 989: 8, 990: 8, 1000: 8, 1001: 8, 1006: 8, 1016: 8, 1026: 8, 1028: 8, 1029: 8, 1030: 8, 1032: 1, 1033: 1, 1034: 8, 1048: 1, 1064: 8, 1070: 8, 1080: 8, 1160: 4, 1281: 8, 1329: 8, 1332: 8, 1335: 8, 1337: 8, 1368: 8, 1412: 8, 1436: 8, 1465: 8, 1476: 8, 1497: 8, 1524: 8, 1527: 8, 1601: 8, 1605: 8, 1611: 8, 1614: 8, 1617: 8, 1621: 8, 1627: 8, 1630: 8, 1800: 4, 1804: 8, 1812: 8, 1815: 8, 1816: 8, 1828: 8, 1831: 8, 1832: 8, 1840: 8, 1848: 8, 1864: 8, 1880: 8, 1892: 8, 1896: 8, 1912: 8, 1960: 8, 1992: 8, 2008: 3, 2043: 5, 2045: 4
@@ -37,6 +35,42 @@ DBC = {
   CAR.AP1_MODELS: dbc_dict('tesla_powertrain', 'tesla_radar', chassis_dbc='tesla_can'),
 }
 
+FW_QUERY_CONFIG = FwQueryConfig(
+  requests=[
+    Request(
+      [StdQueries.TESTER_PRESENT_REQUEST, StdQueries.UDS_VERSION_REQUEST],
+      [StdQueries.TESTER_PRESENT_RESPONSE, StdQueries.UDS_VERSION_RESPONSE],
+      whitelist_ecus=[Ecu.eps],
+      rx_offset=0x08,
+      bus=0,
+    ),
+    Request(
+      [StdQueries.TESTER_PRESENT_REQUEST, StdQueries.UDS_VERSION_REQUEST],
+      [StdQueries.TESTER_PRESENT_RESPONSE, StdQueries.UDS_VERSION_RESPONSE],
+      whitelist_ecus=[Ecu.adas, Ecu.electricBrakeBooster, Ecu.fwdRadar],
+      rx_offset=0x10,
+      bus=0,
+    ),
+  ]
+)
+
+FW_VERSIONS = {
+  CAR.AP2_MODELS: {
+    (Ecu.adas, 0x649, None): [
+      b'\x01\x00\x8b\x07\x01\x00\x01\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x11',
+    ],
+    (Ecu.electricBrakeBooster, 0x64d, None): [
+      b'1037123-00-A',
+    ],
+    (Ecu.fwdRadar, 0x671, None): [
+      b'\x01\x00W\x00\x00\x00\x07\x00\x00\x00\x00\x08\x01\x00\x00\x00\x07\xff\xfe',
+    ],
+    (Ecu.eps, 0x730, None): [
+      b'\x10#\x01',
+    ],
+  },
+}
+
 class CANBUS:
   # Lateral harness
   chassis = 0

selfdrive/car/tests/routes.py

@@ -88,8 +88,8 @@ routes = [
   CarTestRoute("37398f32561a23ad|2021-11-18--00-11-35", HYUNDAI.SANTA_FE_HEV_2022),
   CarTestRoute("656ac0d830792fcc|2021-12-28--14-45-56", HYUNDAI.SANTA_FE_PHEV_2022, segment=1),
   CarTestRoute("e0e98335f3ebc58f|2021-03-07--16-38-29", HYUNDAI.KIA_CEED),
-  CarTestRoute("7653b2bce7bcfdaa|2020-03-04--15-34-32", HYUNDAI.KIA_OPTIMA),
+  CarTestRoute("7653b2bce7bcfdaa|2020-03-04--15-34-32", HYUNDAI.KIA_OPTIMA_G4),
-  CarTestRoute("018654717bc93d7d|2022-09-19--23-11-10", HYUNDAI.KIA_OPTIMA_2019, segment=0),
+  CarTestRoute("018654717bc93d7d|2022-09-19--23-11-10", HYUNDAI.KIA_OPTIMA_G4_FL, segment=0),
   CarTestRoute("c75a59efa0ecd502|2021-03-11--20-52-55", HYUNDAI.KIA_SELTOS),
   CarTestRoute("5b7c365c50084530|2020-04-15--16-13-24", HYUNDAI.SONATA),
   CarTestRoute("b2a38c712dcf90bd|2020-05-18--18-12-48", HYUNDAI.SONATA_LF),
@@ -110,7 +110,8 @@ routes = [
   CarTestRoute("49f3c13141b6bc87|2021-07-28--08-05-13", HYUNDAI.KONA_HEV),
   CarTestRoute("5dddcbca6eb66c62|2020-07-26--13-24-19", HYUNDAI.KIA_STINGER),
   CarTestRoute("d624b3d19adce635|2020-08-01--14-59-12", HYUNDAI.VELOSTER),
-  CarTestRoute("d824e27e8c60172c|2022-05-19--16-15-28", HYUNDAI.KIA_EV6),
+  CarTestRoute("d824e27e8c60172c|2022-05-19--16-15-28", HYUNDAI.KIA_EV6),  # HDA2
+  CarTestRoute("68d6a96e703c00c9|2022-09-10--16-09-39", HYUNDAI.KIA_EV6),  # HDA1
   CarTestRoute("007d5e4ad9f86d13|2021-09-30--15-09-23", HYUNDAI.KIA_K5_2021),
   CarTestRoute("50c6c9b85fd1ff03|2020-10-26--17-56-06", HYUNDAI.KIA_NIRO_EV),
   CarTestRoute("173219cf50acdd7b|2021-07-05--10-27-41", HYUNDAI.KIA_NIRO_PHEV),
@@ -175,6 +176,7 @@ routes = [
   CarTestRoute("4d134e099430fba2|2021-03-26--00-26-06", VOLKSWAGEN.PASSAT_MK8),
   CarTestRoute("3cfdec54aa035f3f|2022-07-19--23-45-10", VOLKSWAGEN.PASSAT_NMS),
   CarTestRoute("0cd0b7f7e31a3853|2021-11-03--19-30-22", VOLKSWAGEN.POLO_MK6),
+  CarTestRoute("064d1816e448f8eb|2022-09-29--15-32-34", VOLKSWAGEN.SHARAN_MK2),
   CarTestRoute("7d82b2f3a9115f1f|2021-10-21--15-39-42", VOLKSWAGEN.TAOS_MK1),
   CarTestRoute("2744c89a8dda9a51|2021-07-24--21-28-06", VOLKSWAGEN.TCROSS_MK1),
   CarTestRoute("2cef8a0b898f331a|2021-03-25--20-13-57", VOLKSWAGEN.TIGUAN_MK2),

selfdrive/car/tests/test_docs.py

@@ -1,4 +1,5 @@
 #!/usr/bin/env python3
+from collections import defaultdict
 import re
 import unittest
 
@@ -20,6 +21,15 @@ class TestCarDocs(unittest.TestCase):
     self.assertEqual(generated_cars_md, current_cars_md,
                      "Run selfdrive/car/docs.py to update the compatibility documentation")
 
+  def test_duplicate_years(self):
+    make_model_years = defaultdict(list)
+    for car in self.all_cars:
+      with self.subTest(car_info_name=car.name):
+        make_model = (car.make, car.model)
+        for year in car.year_list:
+          self.assertNotIn(year, make_model_years[make_model], f"{car.name}: Duplicate model year")
+          make_model_years[make_model].append(year)
+
   def test_missing_car_info(self):
     all_car_info_platforms = get_interface_attr("CAR_INFO", combine_brands=True).keys()
     for platform in sorted(interfaces.keys()):

selfdrive/car/tests/test_fw_fingerprint.py

@@ -44,6 +44,13 @@ class TestFwFingerprint(unittest.TestCase):
             duplicates = {fw for fw in ecu_fw if ecu_fw.count(fw) > 1}
             self.assertFalse(len(duplicates), f"{car_model}: Duplicate FW versions: Ecu.{ECU_NAME[ecu[0]]}, {duplicates}")
 
+  def test_data_collection_ecus(self):
+    for brand, config in FW_QUERY_CONFIGS.items():
+      for car_model, ecus in VERSIONS[brand].items():
+        bad_ecus = set(ecus).intersection(config.extra_ecus)
+        with self.subTest(car_model=car_model):
+          self.assertFalse(len(bad_ecus), f'{car_model}: Fingerprints contain ECUs added for data collection: {bad_ecus}')
+
   def test_blacklisted_ecus(self):
     blacklisted_addrs = (0x7c4, 0x7d0)  # includes A/C ecu and an unknown ecu
     for car_model, ecus in FW_VERSIONS.items():

selfdrive/car/torque_data/override.yaml

@@ -30,6 +30,7 @@ CHEVROLET BOLT EUV 2022: [2.0, 2.0, 0.05]
 CHEVROLET SILVERADO 1500 2020: [1.9, 1.9, 0.112]
 CHEVROLET EQUINOX 2019: [2.0, 2.0, 0.05]
 VOLKSWAGEN PASSAT NMS: [2.5, 2.5, 0.1]
+VOLKSWAGEN SHARAN 2ND GEN: [2.5, 2.5, 0.1]
 HYUNDAI TUCSON HYBRID 4TH GEN: [2.5, 2.5, 0.0]
 
 # Dashcam or fallback configured as ideal car

selfdrive/car/torque_data/substitute.yaml

@@ -17,8 +17,8 @@ LEXUS RC 2020: LEXUS NX 2020
 TOYOTA AVALON HYBRID 2019: TOYOTA AVALON 2019
 TOYOTA AVALON HYBRID 2022: TOYOTA AVALON 2022
 
-KIA OPTIMA 2016: HYUNDAI SONATA 2020
+KIA OPTIMA 4TH GEN: HYUNDAI SONATA 2020
-KIA OPTIMA 2019: HYUNDAI SONATA 2020
+KIA OPTIMA 4TH GEN FACELIFT: HYUNDAI SONATA 2020
 KIA OPTIMA HYBRID 2017 & SPORTS 2019: HYUNDAI SONATA 2020
 KIA FORTE E 2018 & GT 2021: HYUNDAI SONATA 2020
 KIA CEED INTRO ED 2019: HYUNDAI SONATA 2020

selfdrive/car/toyota/values.py

@@ -136,7 +136,7 @@ CAR_INFO: Dict[str, Union[ToyotaCarInfo, List[ToyotaCarInfo]]] = {
   CAR.HIGHLANDERH: ToyotaCarInfo("Toyota Highlander Hybrid 2017-19", footnotes=[Footnote.DSU]),
   CAR.HIGHLANDERH_TSS2: ToyotaCarInfo("Toyota Highlander Hybrid 2020-22"),
   CAR.PRIUS: [
-    ToyotaCarInfo("Toyota Prius 2016", "Toyota Safety Sense P", video_link="https://www.youtube.com/watch?v=8zopPJI8XQ0", footnotes=[Footnote.DSU]),
+    ToyotaCarInfo("Toyota Prius 2016", "Toyota Safety Sense P", "https://www.youtube.com/watch?v=8zopPJI8XQ0", [Footnote.DSU]),
     ToyotaCarInfo("Toyota Prius 2017-20", video_link="https://www.youtube.com/watch?v=8zopPJI8XQ0", footnotes=[Footnote.DSU]),
     ToyotaCarInfo("Toyota Prius Prime 2017-20", video_link="https://www.youtube.com/watch?v=8zopPJI8XQ0", footnotes=[Footnote.DSU]),
   ],
@@ -150,7 +150,7 @@ CAR_INFO: Dict[str, Union[ToyotaCarInfo, List[ToyotaCarInfo]]] = {
     ToyotaCarInfo("Toyota RAV4 2017-18", footnotes=[Footnote.DSU])
   ],
   CAR.RAV4H: [
-    ToyotaCarInfo("Toyota RAV4 Hybrid 2016", "Toyota Safety Sense P", video_link="https://youtu.be/LhT5VzJVfNI?t=26", footnotes=[Footnote.DSU]),
+    ToyotaCarInfo("Toyota RAV4 Hybrid 2016", "Toyota Safety Sense P", "https://youtu.be/LhT5VzJVfNI?t=26", [Footnote.DSU]),
     ToyotaCarInfo("Toyota RAV4 Hybrid 2017-18", video_link="https://youtu.be/LhT5VzJVfNI?t=26", footnotes=[Footnote.DSU])
   ],
   CAR.RAV4_TSS2: ToyotaCarInfo("Toyota RAV4 2019-21", video_link="https://www.youtube.com/watch?v=wJxjDd42gGA"),
@@ -171,8 +171,14 @@ CAR_INFO: Dict[str, Union[ToyotaCarInfo, List[ToyotaCarInfo]]] = {
   CAR.LEXUS_NX_TSS2: ToyotaCarInfo("Lexus NX 2020-21"),
   CAR.LEXUS_NXH_TSS2: ToyotaCarInfo("Lexus NX Hybrid 2020-21"),
   CAR.LEXUS_RC: ToyotaCarInfo("Lexus RC 2017-20"),
-  CAR.LEXUS_RX: ToyotaCarInfo("Lexus RX 2016-19", footnotes=[Footnote.DSU]),
+  CAR.LEXUS_RX: [
-  CAR.LEXUS_RXH: ToyotaCarInfo("Lexus RX Hybrid 2016-19", footnotes=[Footnote.DSU]),
+    ToyotaCarInfo("Lexus RX 2016", "Lexus Safety System+", footnotes=[Footnote.DSU]),
+    ToyotaCarInfo("Lexus RX 2017-19", footnotes=[Footnote.DSU]),
+  ],
+  CAR.LEXUS_RXH: [
+    ToyotaCarInfo("Lexus RX Hybrid 2016", "Lexus Safety System+", footnotes=[Footnote.DSU]),
+    ToyotaCarInfo("Lexus RX Hybrid 2017-19", footnotes=[Footnote.DSU]),
+  ],
   CAR.LEXUS_RX_TSS2: ToyotaCarInfo("Lexus RX 2020-22"),
   CAR.LEXUS_RXH_TSS2: ToyotaCarInfo("Lexus RX Hybrid 2020-21"),
 }
@@ -751,6 +757,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'\x03312N6200\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00895231203202\x00\x00\x00\x00',
       b'\x03312N6200\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00895231203302\x00\x00\x00\x00',
       b'\x02312K4000\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00',
       b'\x02312U5000\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00',
@@ -1356,6 +1363,7 @@ FW_VERSIONS = {
     ],
     (Ecu.engine, 0x700, None): [
       b'\x01896634AA0000\x00\x00\x00\x00',
+      b'\x01896634AA0100\x00\x00\x00\x00',      
       b'\x01896634AA1000\x00\x00\x00\x00',
       b'\x01896634A88000\x00\x00\x00\x00',
       b'\x01896634A89000\x00\x00\x00\x00',

selfdrive/car/vin.py

@@ -1,8 +1,8 @@
 #!/usr/bin/env python3
 import re
-import traceback
 
 import cereal.messaging as messaging
+from panda.python.uds import get_rx_addr_for_tx_addr, FUNCTIONAL_ADDRS
 from selfdrive.car.isotp_parallel_query import IsoTpParallelQuery
 from selfdrive.car.fw_query_definitions import StdQueries
 from system.swaglog import cloudlog
@@ -16,29 +16,48 @@ def is_valid_vin(vin: str):
 
 
 def get_vin(logcan, sendcan, bus, timeout=0.1, retry=5, debug=False):
-  addrs = [0x7e0, 0x7e2, 0x18da10f1, 0x18da0ef1]  # engine, VMCU, 29-bit engine, PGM-FI
+  addrs = list(range(0x7e0, 0x7e8)) + list(range(0x18DA00F1, 0x18DB00F1, 0x100))  # addrs to process/wait for
+  valid_vin_addrs = [0x7e0, 0x7e2, 0x18da10f1, 0x18da0ef1]  # engine, VMCU, 29-bit engine, PGM-FI
   for i in range(retry):
     for request, response in ((StdQueries.UDS_VIN_REQUEST, StdQueries.UDS_VIN_RESPONSE), (StdQueries.OBD_VIN_REQUEST, StdQueries.OBD_VIN_RESPONSE)):
       try:
-        query = IsoTpParallelQuery(sendcan, logcan, bus, addrs, [request, ], [response, ], debug=debug)
+        query = IsoTpParallelQuery(sendcan, logcan, bus, addrs, [request, ], [response, ], functional_addrs=FUNCTIONAL_ADDRS, debug=debug)
-        for (addr, rx_addr), vin in query.get_data(timeout).items():
+        results = query.get_data(timeout)
+
+        for addr in valid_vin_addrs:
+          vin = results.get((addr, None))
+          if vin is not None:
+            # Ford pads with null bytes
+            if len(vin) == 24:
+              vin = re.sub(b'\x00*$', b'', vin)
 
             # Honda Bosch response starts with a length, trim to correct length
             if vin.startswith(b'\x11'):
               vin = vin[1:18]
 
-          return addr[0], rx_addr, vin.decode()
+            return get_rx_addr_for_tx_addr(addr), vin.decode()
-        print(f"vin query retry ({i+1}) ...")
+
+        cloudlog.error(f"vin query retry ({i+1}) ...")
       except Exception:
-        cloudlog.warning(f"VIN query exception: {traceback.format_exc()}")
+        cloudlog.exception("VIN query exception")
 
-  return 0, 0, VIN_UNKNOWN
+  return 0, VIN_UNKNOWN
 
 
 if __name__ == "__main__":
+  import argparse
   import time
+
+  parser = argparse.ArgumentParser(description='Get VIN of the car')
+  parser.add_argument('--debug', action='store_true')
+  parser.add_argument('--bus', type=int, default=1)
+  parser.add_argument('--timeout', type=float, default=0.1)
+  parser.add_argument('--retry', type=int, default=5)
+  args = parser.parse_args()
+
   sendcan = messaging.pub_sock('sendcan')
   logcan = messaging.sub_sock('can')
   time.sleep(1)
-  addr, vin_rx_addr, vin = get_vin(logcan, sendcan, 1, debug=False)
+
-  print(f'TX: {hex(addr)}, RX: {hex(vin_rx_addr)}, VIN: {vin}')
+  vin_rx_addr, vin = get_vin(logcan, sendcan, args.bus, args.timeout, args.retry, debug=args.debug)
+  print(f'RX: {hex(vin_rx_addr)}, VIN: {vin}')

selfdrive/car/volkswagen/carcontroller.py

@@ -76,7 +76,7 @@ class CarController:
       stopping = actuators.longControlState == LongCtrlState.stopping
       starting = actuators.longControlState == LongCtrlState.starting
       can_sends.extend(self.CCS.create_acc_accel_control(self.packer_pt, CANBUS.pt, CS.acc_type, CC.longActive, accel,
-                                                         acc_control, stopping, starting, CS.out.cruiseState.standstill))
+                                                         acc_control, stopping, starting, CS.esp_hold_confirmation))
 
     # **** HUD Controls ***************************************************** #
 

selfdrive/car/volkswagen/carstate.py

@@ -110,20 +110,21 @@ class CarState(CarStateBase):
       ret.cruiseState.available = True
       ret.cruiseState.enabled = False
     elif pt_cp.vl["TSK_06"]["TSK_Status"] in (3, 4, 5):
-      # ACC okay and enabled, currently regulating speed (3) or driver accel override (4) or overrun coast-down (5)
+      # ACC okay and enabled, currently regulating speed (3) or driver accel override (4) or brake only (5)
       ret.cruiseState.available = True
       ret.cruiseState.enabled = True
     else:
       # ACC okay but disabled (1), or a radar visibility or other fault/disruption (6 or 7)
       ret.cruiseState.available = False
       ret.cruiseState.enabled = False
-    ret.cruiseState.standstill = bool(pt_cp.vl["ESP_21"]["ESP_Haltebestaetigung"])
+    self.esp_hold_confirmation = bool(pt_cp.vl["ESP_21"]["ESP_Haltebestaetigung"])
+    ret.cruiseState.standstill = self.CP.pcmCruise and self.esp_hold_confirmation
     ret.accFaulted = pt_cp.vl["TSK_06"]["TSK_Status"] in (6, 7)
 
     # Update ACC setpoint. When the setpoint is zero or there's an error, the
     # radar sends a set-speed of ~90.69 m/s / 203mph.
     if self.CP.pcmCruise:
-      ret.cruiseState.speed = ext_cp.vl["ACC_02"]["ACC_Wunschgeschw"] * CV.KPH_TO_MS
+      ret.cruiseState.speed = ext_cp.vl["ACC_02"]["ACC_Wunschgeschw_02"] * CV.KPH_TO_MS
       if ret.cruiseState.speed > 90:
         ret.cruiseState.speed = 0
 
@@ -170,7 +171,7 @@ class CarState(CarStateBase):
     ret.gas = pt_cp.vl["Motor_3"]["Fahrpedal_Rohsignal"] / 100.0
     ret.gasPressed = ret.gas > 0
     ret.brake = pt_cp.vl["Bremse_5"]["Bremsdruck"] / 250.0  # FIXME: this is pressure in Bar, not sure what OP expects
-    ret.brakePressed = bool(pt_cp.vl["Motor_2"]["Bremstestschalter"])
+    ret.brakePressed = bool(pt_cp.vl["Motor_2"]["Bremslichtschalter"])
     ret.parkingBrake = bool(pt_cp.vl["Kombi_1"]["Bremsinfo"])
 
     # Update gear and/or clutch position data.
@@ -478,7 +479,7 @@ class CarState(CarStateBase):
 class MqbExtraSignals:
   # Additional signal and message lists for optional or bus-portable controllers
   fwd_radar_signals = [
-    ("ACC_Wunschgeschw", "ACC_02"),              # ACC set speed
+    ("ACC_Wunschgeschw_02", "ACC_02"),           # ACC set speed
     ("AWV2_Freigabe", "ACC_10"),                 # FCW brake jerk release
     ("ANB_Teilbremsung_Freigabe", "ACC_10"),     # AEB partial braking release
     ("ANB_Zielbremsung_Freigabe", "ACC_10"),     # AEB target braking release

selfdrive/car/volkswagen/interface.py

@@ -6,6 +6,7 @@ from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness,
 from selfdrive.car.interfaces import CarInterfaceBase
 from selfdrive.car.volkswagen.values import CAR, PQ_CARS, CANBUS, NetworkLocation, TransmissionType, GearShifter
 
+ButtonType = car.CarState.ButtonEvent.Type
 EventName = car.CarEvent.EventName
 
 
@@ -127,6 +128,13 @@ class CarInterface(CarInterfaceBase):
       ret.mass = 1230 + STD_CARGO_KG
       ret.wheelbase = 2.55
 
+    elif candidate == CAR.SHARAN_MK2:
+      ret.mass = 1639 + STD_CARGO_KG
+      ret.wheelbase = 2.92
+      ret.minEnableSpeed = 30 * CV.KPH_TO_MS
+      ret.minSteerSpeed = 50 * CV.KPH_TO_MS
+      ret.steerActuatorDelay = 0.2
+
     elif candidate == CAR.TAOS_MK1:
       ret.mass = 1498 + STD_CARGO_KG
       ret.wheelbase = 2.69
@@ -211,7 +219,8 @@ class CarInterface(CarInterfaceBase):
     ret = self.CS.update(self.cp, self.cp_cam, self.cp_ext, self.CP.transmissionType)
 
     events = self.create_common_events(ret, extra_gears=[GearShifter.eco, GearShifter.sport, GearShifter.manumatic],
-                                       pcm_enable=not self.CS.CP.openpilotLongitudinalControl)
+                                       pcm_enable=not self.CS.CP.openpilotLongitudinalControl,
+                                       enable_buttons=(ButtonType.setCruise, ButtonType.resumeCruise))
 
     # Low speed steer alert hysteresis logic
     if self.CP.minSteerSpeed > 0. and ret.vEgo < (self.CP.minSteerSpeed + 1.):

selfdrive/car/volkswagen/pqcan.py

@@ -59,7 +59,7 @@ def acc_hud_status_value(main_switch_on, acc_faulted, long_active):
   return hud_status
 
 
-def create_acc_accel_control(packer, bus, acc_type, enabled, accel, acc_control, stopping, starting, standstill):
+def create_acc_accel_control(packer, bus, acc_type, enabled, accel, acc_control, stopping, starting, esp_hold):
   commands = []
 
   values = {

selfdrive/car/volkswagen/values.py

@@ -116,6 +116,7 @@ class CAR:
   PASSAT_MK8 = "VOLKSWAGEN PASSAT 8TH GEN"          # Chassis 3G, Mk8 VW Passat and variants
   PASSAT_NMS = "VOLKSWAGEN PASSAT NMS"              # Chassis A3, North America/China/Mideast NMS Passat, incl. facelift
   POLO_MK6 = "VOLKSWAGEN POLO 6TH GEN"              # Chassis AW, Mk6 VW Polo
+  SHARAN_MK2 = "VOLKSWAGEN SHARAN 2ND GEN"          # Chassis 7N, Mk2 Volkswagen Sharan and SEAT Alhambra
   TAOS_MK1 = "VOLKSWAGEN TAOS 1ST GEN"              # Chassis B2, Mk1 VW Taos and Tharu
   TCROSS_MK1 = "VOLKSWAGEN T-CROSS 1ST GEN"         # Chassis C1, Mk1 VW T-Cross SWB and LWB variants
   TIGUAN_MK2 = "VOLKSWAGEN TIGUAN 2ND GEN"          # Chassis AD/BW, Mk2 VW Tiguan and variants
@@ -135,7 +136,7 @@ class CAR:
   SKODA_OCTAVIA_MK3 = "SKODA OCTAVIA 3RD GEN"       # Chassis NE, Mk3 Skoda Octavia and variants
 
 
-PQ_CARS = {CAR.PASSAT_NMS}
+PQ_CARS = {CAR.PASSAT_NMS, CAR.SHARAN_MK2}
 
 
 DBC: Dict[str, Dict[str, str]] = defaultdict(lambda: dbc_dict("vw_mqb_2010", None))
@@ -206,6 +207,10 @@ CAR_INFO: Dict[str, Union[VWCarInfo, List[VWCarInfo]]] = {
     VWCarInfo("Volkswagen Polo 2020-22", footnotes=[Footnote.VW_HARNESS], harness=Harness.j533),
     VWCarInfo("Volkswagen Polo GTI 2020-22", footnotes=[Footnote.VW_HARNESS], harness=Harness.j533),
   ],
+  CAR.SHARAN_MK2: [
+    VWCarInfo("Volkswagen Sharan 2018-22", footnotes=[Footnote.VW_HARNESS], harness=Harness.j533),
+    VWCarInfo("SEAT Alhambra 2018-20", footnotes=[Footnote.VW_HARNESS], harness=Harness.j533),
+  ],
   CAR.TAOS_MK1: VWCarInfo("Volkswagen Taos 2022", footnotes=[Footnote.VW_HARNESS], harness=Harness.j533),
   CAR.TCROSS_MK1: VWCarInfo("Volkswagen T-Cross 2021", footnotes=[Footnote.VW_HARNESS], harness=Harness.j533),
   CAR.TIGUAN_MK2: VWCarInfo("Volkswagen Tiguan 2019-22", footnotes=[Footnote.VW_HARNESS], harness=Harness.j533),
@@ -236,6 +241,7 @@ CAR_INFO: Dict[str, Union[VWCarInfo, List[VWCarInfo]]] = {
   ],
 }
 
+
 # All supported cars should return FW from the engine, srs, eps, and fwdRadar. Cars
 # with a manual trans won't return transmission firmware, but all other cars will.
 #
@@ -625,6 +631,18 @@ FW_VERSIONS = {
       b'\xf1\x872Q0907572R \xf1\x890372',
     ],
   },
+  CAR.SHARAN_MK2: {
+    # TODO: Sharan Mk2 EPS and DQ250 auto trans both require KWP2000 support for fingerprinting
+    (Ecu.engine, 0x7e0, None): [
+      b'\xf1\x8704L906016HE\xf1\x894635',
+    ],
+    (Ecu.srs, 0x715, None): [
+      b'\xf1\x877N0959655D \xf1\x890016\xf1\x82\x0801100705----10--',
+    ],
+    (Ecu.fwdRadar, 0x757, None): [
+      b'\xf1\x877N0907572C \xf1\x890211\xf1\x82\x0153',
+    ],
+  },
   CAR.TAOS_MK1: {
     (Ecu.engine, 0x7e0, None): [
       b'\xf1\x8704E906027NJ\xf1\x891445',

selfdrive/controls/controlsd.py

@@ -143,6 +143,12 @@ class Controls:
     put_nonblocking("CarParamsCache", cp_bytes)
     put_nonblocking("CarParamsPersistent", cp_bytes)
 
+    # cleanup old params
+    if not self.CP.experimentalLongitudinalAvailable:
+      params.remove("ExperimentalLongitudinalEnabled")
+    if not self.CP.openpilotLongitudinalControl:
+      params.remove("EndToEndLong")
+
     self.CC = car.CarControl.new_message()
     self.CS_prev = car.CarState.new_message()
     self.AM = AlertManager()
@@ -313,7 +319,7 @@ class Controls:
       else:
         safety_mismatch = pandaState.safetyModel not in IGNORED_SAFETY_MODES
 
-      if safety_mismatch or self.mismatch_counter >= 200:
+      if safety_mismatch or pandaState.safetyRxChecksInvalid or self.mismatch_counter >= 200:
         self.events.add(EventName.controlsMismatch)
 
       if log.PandaState.FaultType.relayMalfunction in pandaState.faults:
@@ -699,6 +705,7 @@ class Controls:
     if len(angular_rate_value) > 2:
       CC.angularVelocity = angular_rate_value
 
+    CC.cruiseControl.override = self.enabled and not CC.longActive and self.CP.openpilotLongitudinalControl
     CC.cruiseControl.cancel = CS.cruiseState.enabled and (not self.enabled or not self.CP.pcmCruise)
     if self.joystick_mode and self.sm.rcv_frame['testJoystick'] > 0 and self.sm['testJoystick'].buttons[0]:
       CC.cruiseControl.cancel = True

selfdrive/controls/lib/drive_helpers.py

@@ -33,12 +33,6 @@ CRUISE_INTERVAL_SIGN = {
 }
 
 
-class MPC_COST_LAT:
-  PATH = 1.0
-  HEADING = 1.0
-  STEER_RATE = 1.0
-
-
 def apply_deadzone(error, deadzone):
   if error > deadzone:
     error -= deadzone

selfdrive/controls/lib/events.py

@@ -811,6 +811,10 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = {
     ET.NO_ENTRY: NoEntryAlert("Cruise Faulted"),
   },
 
+  EventName.gmAccFaultedTemp: {
+    ET.NO_ENTRY: NoEntryAlert("Cruise Temporarily Faulted"),
+  },
+
   EventName.controlsMismatch: {
     ET.IMMEDIATE_DISABLE: ImmediateDisableAlert("Controls Mismatch"),
     ET.NO_ENTRY: NoEntryAlert("Controls Mismatch"),

selfdrive/controls/lib/latcontrol_torque.py

@@ -17,6 +17,8 @@ from selfdrive.controls.lib.vehicle_model import ACCELERATION_DUE_TO_GRAVITY
 # friction in the steering wheel that needs to be overcome to
 # move it at all, this is compensated for too.
 
+LOW_SPEED_FACTOR = 100
+
 
 class LatControlTorque(LatControl):
   def __init__(self, CP, CI):
@@ -55,13 +57,15 @@ class LatControlTorque(LatControl):
       actual_lateral_accel = actual_curvature * CS.vEgo ** 2
       lateral_accel_deadzone = curvature_deadzone * CS.vEgo ** 2
 
-      low_speed_factor = interp(CS.vEgo, [0, 10, 20], [500, 500, 200])
+      setpoint = desired_lateral_accel + LOW_SPEED_FACTOR * desired_curvature
-      setpoint = desired_lateral_accel + low_speed_factor * desired_curvature
+      measurement = actual_lateral_accel + LOW_SPEED_FACTOR * actual_curvature
-      measurement = actual_lateral_accel + low_speed_factor * actual_curvature
       error = setpoint - measurement
       gravity_adjusted_lateral_accel = desired_lateral_accel - params.roll * ACCELERATION_DUE_TO_GRAVITY
-      pid_log.error = self.torque_from_lateral_accel(error, self.torque_params, error, lateral_accel_deadzone, friction_compensation=False)
+      pid_log.error = self.torque_from_lateral_accel(error, self.torque_params, error,
-      ff = self.torque_from_lateral_accel(gravity_adjusted_lateral_accel, self.torque_params, error, lateral_accel_deadzone, friction_compensation=True)
+                                                     lateral_accel_deadzone, friction_compensation=False)
+      ff = self.torque_from_lateral_accel(gravity_adjusted_lateral_accel, self.torque_params,
+                                          desired_lateral_accel - actual_lateral_accel,
+                                          lateral_accel_deadzone, friction_compensation=True)
 
       freeze_integrator = steer_limited or CS.steeringPressed or CS.vEgo < 5
       output_torque = self.pid.update(pid_log.error,

selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py

@@ -5,7 +5,6 @@ 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.modeld.constants import T_IDXS
 
 if __name__ == '__main__':  # generating code
@@ -18,6 +17,10 @@ EXPORT_DIR = os.path.join(LAT_MPC_DIR, "c_generated_code")
 JSON_FILE = os.path.join(LAT_MPC_DIR, "acados_ocp_lat.json")
 X_DIM = 4
 P_DIM = 2
+N = 16
+COST_E_DIM = 3
+COST_DIM = COST_E_DIM + 2
+SPEED_OFFSET = 10.0
 MODEL_NAME = 'lat'
 ACADOS_SOLVER_TYPE = 'SQP_RTI'
 
@@ -29,8 +32,8 @@ def gen_lat_model():
   x_ego = SX.sym('x_ego')
   y_ego = SX.sym('y_ego')
   psi_ego = SX.sym('psi_ego')
-  curv_ego = SX.sym('curv_ego')
+  psi_rate_ego = SX.sym('psi_rate_ego')
-  model.x = vertcat(x_ego, y_ego, psi_ego, curv_ego)
+  model.x = vertcat(x_ego, y_ego, psi_ego, psi_rate_ego)
 
   # parameters
   v_ego = SX.sym('v_ego')
@@ -38,22 +41,22 @@ def gen_lat_model():
   model.p = vertcat(v_ego, rotation_radius)
 
   # controls
-  curv_rate = SX.sym('curv_rate')
+  psi_accel_ego = SX.sym('psi_accel_ego')
-  model.u = vertcat(curv_rate)
+  model.u = vertcat(psi_accel_ego)
 
   # xdot
   x_ego_dot = SX.sym('x_ego_dot')
   y_ego_dot = SX.sym('y_ego_dot')
   psi_ego_dot = SX.sym('psi_ego_dot')
-  curv_ego_dot = SX.sym('curv_ego_dot')
+  psi_rate_ego_dot = SX.sym('psi_rate_ego_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, psi_rate_ego_dot)
 
   # dynamics model
-  f_expl = vertcat(v_ego * cos(psi_ego) - rotation_radius * sin(psi_ego) * (v_ego * curv_ego),
+  f_expl = vertcat(v_ego * cos(psi_ego) - rotation_radius * sin(psi_ego) * psi_rate_ego,
-                   v_ego * sin(psi_ego) + rotation_radius * cos(psi_ego) * (v_ego * curv_ego),
+                   v_ego * sin(psi_ego) + rotation_radius * cos(psi_ego) * psi_rate_ego,
-                   v_ego * curv_ego,
+                   psi_rate_ego,
-                   curv_rate)
+                   psi_accel_ego)
   model.f_impl_expr = model.xdot - f_expl
   model.f_expl_expr = f_expl
   return model
@@ -72,26 +75,35 @@ def gen_lat_ocp():
   ocp.cost.cost_type = 'NONLINEAR_LS'
   ocp.cost.cost_type_e = 'NONLINEAR_LS'
 
-  Q = np.diag([0.0, 0.0])
+  Q = np.diag(np.zeros(COST_E_DIM))
-  QR = np.diag([0.0, 0.0, 0.0])
+  QR = np.diag(np.zeros(COST_DIM))
 
   ocp.cost.W = QR
   ocp.cost.W_e = Q
 
-  y_ego, psi_ego = ocp.model.x[1], ocp.model.x[2]
+  y_ego, psi_ego, psi_rate_ego = ocp.model.x[1], ocp.model.x[2], ocp.model.x[3]
-  curv_rate = ocp.model.u[0]
+  psi_rate_ego_dot = ocp.model.u[0]
   v_ego = ocp.model.p[0]
 
   ocp.parameter_values = np.zeros((P_DIM, ))
 
-  ocp.cost.yref = np.zeros((3, ))
+  ocp.cost.yref = np.zeros((COST_DIM, ))
-  ocp.cost.yref_e = np.zeros((2, ))
+  ocp.cost.yref_e = np.zeros((COST_E_DIM, ))
-  # TODO hacky weights to keep behavior the same
+  # Add offset to smooth out low speed control
+  # TODO unclear if this right solution long term
+  v_ego_offset = v_ego + SPEED_OFFSET
+  # TODO there are two costs on psi_rate_ego_dot, one
+  # is correlated to jerk the other to steering wheel movement
+  # the steering wheel movement cost is added to prevent excessive
+  # wheel movements
   ocp.model.cost_y_expr = vertcat(y_ego,
-                                  ((v_ego +5.0) * psi_ego),
+                                  v_ego_offset * psi_ego,
-                                  ((v_ego + 5.0) * 4.0 * curv_rate))
+                                  v_ego_offset * psi_rate_ego,
+                                  v_ego_offset * psi_rate_ego_dot,
+                                  psi_rate_ego_dot / (v_ego + 0.1))
   ocp.model.cost_y_expr_e = vertcat(y_ego,
-                                    ((v_ego +5.0) * psi_ego))
+                                   v_ego_offset * psi_ego,
+                                   v_ego_offset * psi_rate_ego)
 
   # set constraints
   ocp.constraints.constr_type = 'BGH'
@@ -124,10 +136,10 @@ class LateralMpc():
   def reset(self, x0=np.zeros(X_DIM)):
     self.x_sol = np.zeros((N+1, X_DIM))
     self.u_sol = np.zeros((N, 1))
-    self.yref = np.zeros((N+1, 3))
+    self.yref = np.zeros((N+1, COST_DIM))
     for i in range(N):
       self.solver.cost_set(i, "yref", self.yref[i])
-    self.solver.cost_set(N, "yref", self.yref[N][:2])
+    self.solver.cost_set(N, "yref", self.yref[N][:COST_E_DIM])
 
     # Somehow needed for stable init
     for i in range(N+1):
@@ -140,14 +152,17 @@ class LateralMpc():
     self.solve_time = 0.0
     self.cost = 0
 
-  def set_weights(self, path_weight, heading_weight, steer_rate_weight):
+  def set_weights(self, path_weight, heading_weight,
-    W = np.asfortranarray(np.diag([path_weight, heading_weight, steer_rate_weight]))
+                  lat_accel_weight, lat_jerk_weight,
+                  steering_rate_weight):
+    W = np.asfortranarray(np.diag([path_weight, heading_weight,
+                                   lat_accel_weight, lat_jerk_weight,
+                                   steering_rate_weight]))
     for i in range(N):
       self.solver.cost_set(i, 'W', W)
-    #TODO hacky weights to keep behavior the same
+    self.solver.cost_set(N, 'W', W[:COST_E_DIM,:COST_E_DIM])
-    self.solver.cost_set(N, 'W', (3/20.)*W[:2,:2])
 
-  def run(self, x0, p, y_pts, heading_pts, curv_rate_pts):
+  def run(self, x0, p, y_pts, heading_pts, yaw_rate_pts):
     x0_cp = np.copy(x0)
     p_cp = np.copy(p)
     self.solver.constraints_set(0, "lbx", x0_cp)
@@ -155,13 +170,13 @@ class LateralMpc():
     self.yref[:,0] = y_pts
     v_ego = p_cp[0]
     # rotation_radius = p_cp[1]
-    self.yref[:,1] = heading_pts*(v_ego+5.0)
+    self.yref[:,1] = heading_pts * (v_ego + SPEED_OFFSET)
-    self.yref[:,2] = curv_rate_pts * (v_ego+5.0) * 4.0
+    self.yref[:,2] = yaw_rate_pts * (v_ego + SPEED_OFFSET)
     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])
+    self.solver.cost_set(N, "yref", self.yref[N][:COST_E_DIM])
 
     t = sec_since_boot()
     self.solution_status = self.solver.solve()

selfdrive/controls/lib/lateral_planner.py

@@ -3,7 +3,8 @@ from common.realtime import sec_since_boot, DT_MDL
 from common.numpy_fast import interp
 from system.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
+from selfdrive.controls.lib.lateral_mpc_lib.lat_mpc import N as LAT_MPC_N
+from selfdrive.controls.lib.drive_helpers import CONTROL_N
 from selfdrive.controls.lib.desire_helper import DesireHelper
 import cereal.messaging as messaging
 from cereal import log
@@ -11,6 +12,20 @@ from cereal import log
 TRAJECTORY_SIZE = 33
 CAMERA_OFFSET = 0.04
 
+
+PATH_COST = 1.0
+LATERAL_MOTION_COST = 0.11
+LATERAL_ACCEL_COST = 0.0
+LATERAL_JERK_COST = 0.05
+# Extreme steering rate is unpleasant, even
+# when it does not cause bad jerk.
+# TODO this cost should be lowered when low
+# speed lateral control is stable on all cars
+STEERING_RATE_COST = 800.0
+
+MIN_SPEED = .3
+
+
 class LateralPlanner:
   def __init__(self, CP):
     self.DH = DesireHelper()
@@ -22,9 +37,8 @@ class LateralPlanner:
     self.solution_invalid_cnt = 0
 
     self.path_xyz = np.zeros((TRAJECTORY_SIZE, 3))
-    self.path_xyz_stds = np.ones((TRAJECTORY_SIZE, 3))
     self.plan_yaw = np.zeros((TRAJECTORY_SIZE,))
-    self.plan_curv_rate = np.zeros((TRAJECTORY_SIZE,))
+    self.plan_yaw_rate = np.zeros((TRAJECTORY_SIZE,))
     self.t_idxs = np.arange(TRAJECTORY_SIZE)
     self.y_pts = np.zeros(TRAJECTORY_SIZE)
 
@@ -36,7 +50,8 @@ class LateralPlanner:
     self.lat_mpc.reset(x0=self.x0)
 
   def update(self, sm):
-    v_ego = sm['carState'].vEgo
+    # clip speed , lateral planning is not possible at 0 speed
+    self.v_ego = max(MIN_SPEED, sm['carState'].vEgo)
     measured_curvature = sm['controlsState'].curvature
 
     # Parse model predictions
@@ -45,8 +60,7 @@ class LateralPlanner:
       self.path_xyz = np.column_stack([md.position.x, md.position.y, md.position.z])
       self.t_idxs = np.array(md.position.t)
       self.plan_yaw = np.array(md.orientation.z)
-    if len(md.position.xStd) == TRAJECTORY_SIZE:
+      self.plan_yaw_rate = np.array(md.orientationRate.z)
-      self.path_xyz_stds = np.column_stack([md.position.xStd, md.position.yStd, md.position.zStd])
 
     # Lane change logic
     desire_state = md.meta.desireState
@@ -57,29 +71,29 @@ class LateralPlanner:
     self.DH.update(sm['carState'], sm['carControl'].latActive, lane_change_prob)
 
     d_path_xyz = self.path_xyz
-    # Heading cost is useful at low speed, otherwise end of plan can be off-heading
+    self.lat_mpc.set_weights(PATH_COST, LATERAL_MOTION_COST,
-    heading_cost = interp(v_ego, [5.0, 10.0], [MPC_COST_LAT.HEADING, 0.15])
+                             LATERAL_ACCEL_COST, LATERAL_JERK_COST,
-    self.lat_mpc.set_weights(MPC_COST_LAT.PATH, heading_cost, MPC_COST_LAT.STEER_RATE)
+                             STEERING_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])
+    y_pts = np.interp(self.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)
+    heading_pts = np.interp(self.v_ego * self.t_idxs[:LAT_MPC_N + 1], np.linalg.norm(self.path_xyz, axis=1), self.plan_yaw)
-    curv_rate_pts = np.interp(v_ego * self.t_idxs[:LAT_MPC_N + 1], np.linalg.norm(self.path_xyz, axis=1), self.plan_curv_rate)
+    yaw_rate_pts = np.interp(self.v_ego * self.t_idxs[:LAT_MPC_N + 1], np.linalg.norm(self.path_xyz, axis=1), self.plan_yaw_rate)
     self.y_pts = y_pts
 
     assert len(y_pts) == LAT_MPC_N + 1
     assert len(heading_pts) == LAT_MPC_N + 1
-    assert len(curv_rate_pts) == LAT_MPC_N + 1
+    assert len(yaw_rate_pts) == LAT_MPC_N + 1
-    lateral_factor = max(0, self.factor1 - (self.factor2 * v_ego**2))
+    lateral_factor = max(0, self.factor1 - (self.factor2 * self.v_ego**2))
-    p = np.array([v_ego, lateral_factor])
+    p = np.array([self.v_ego, lateral_factor])
     self.lat_mpc.run(self.x0,
                      p,
                      y_pts,
                      heading_pts,
-                     curv_rate_pts)
+                     yaw_rate_pts)
-    # init state for next
+    # init state for next iteration
-    # mpc.u_sol is the desired curvature rate given x0 curv state.
+    # mpc.u_sol is the desired second derivative of psi given x0 curv state.
-    # with x0[3] = measured_curvature, this would be the actual desired rate.
+    # with x0[3] = measured_yaw_rate, this would be the actual desired yaw rate.
-    # instead, interpolate x_sol so that x0[3] is the desired curvature for lat_control.
+    # instead, interpolate x_sol so that x0[3] is the desired yaw rate for lat_control.
     self.x0[3] = interp(DT_MDL, self.t_idxs[:LAT_MPC_N + 1], self.lat_mpc.x_sol[:, 3])
 
     #  Check for infeasible MPC solution
@@ -87,7 +101,7 @@ class LateralPlanner:
     t = sec_since_boot()
     if mpc_nans or self.lat_mpc.solution_status != 0:
       self.reset_mpc()
-      self.x0[3] = measured_curvature
+      self.x0[3] = measured_curvature * self.v_ego
       if t > self.last_cloudlog_t + 5.0:
         self.last_cloudlog_t = t
         cloudlog.warning("Lateral mpc - nan: True")
@@ -106,8 +120,9 @@ class LateralPlanner:
     lateralPlan.modelMonoTime = sm.logMonoTime['modelV2']
     lateralPlan.dPathPoints = self.y_pts.tolist()
     lateralPlan.psis = self.lat_mpc.x_sol[0:CONTROL_N, 2].tolist()
-    lateralPlan.curvatures = self.lat_mpc.x_sol[0:CONTROL_N, 3].tolist()
+
-    lateralPlan.curvatureRates = [float(x) for x in self.lat_mpc.u_sol[0:CONTROL_N - 1]] + [0.0]
+    lateralPlan.curvatures = (self.lat_mpc.x_sol[0:CONTROL_N, 3]/self.v_ego).tolist()
+    lateralPlan.curvatureRates = [float(x/self.v_ego) for x in self.lat_mpc.u_sol[0:CONTROL_N - 1]] + [0.0]
 
     lateralPlan.mpcSolutionValid = bool(plan_solution_valid)
     lateralPlan.solverExecutionTime = self.lat_mpc.solve_time

selfdrive/controls/lib/longcontrol.py

@@ -7,9 +7,6 @@ from selfdrive.modeld.constants import T_IDXS
 
 LongCtrlState = car.CarControl.Actuators.LongControlState
 
-# As per ISO 15622:2018 for all speeds
-ACCEL_MIN_ISO = -3.5  # m/s^2
-ACCEL_MAX_ISO = 2.0  # m/s^2
 
 def long_control_state_trans(CP, active, long_control_state, v_ego, v_target,
                              v_target_1sec, brake_pressed, cruise_standstill):
@@ -50,10 +47,6 @@ def long_control_state_trans(CP, active, long_control_state, v_ego, v_target,
       elif started_condition:
         long_control_state = LongCtrlState.pid
 
-
-
-
-
   return long_control_state
 
 

selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py

@@ -253,11 +253,8 @@ class LongitudinalMpc:
       cost_weights = [X_EGO_OBSTACLE_COST, X_EGO_COST, V_EGO_COST, A_EGO_COST, a_change_cost, J_EGO_COST]
       constraint_cost_weights = [LIMIT_COST, LIMIT_COST, LIMIT_COST, DANGER_ZONE_COST]
     elif self.mode == 'blended':
-      cost_weights = [0., 0.2, 0.25, 1.0, 0.0, 1.0]
+      cost_weights = [0., 0.1, 0.2, 5.0, 0.0, 1.0]
       constraint_cost_weights = [LIMIT_COST, LIMIT_COST, LIMIT_COST, 50.0]
-    elif self.mode == 'e2e':
-      cost_weights = [0., 0.2, 0.25, 1., 0.0, .1]
-      constraint_cost_weights = [LIMIT_COST, LIMIT_COST, LIMIT_COST, 0.0]
     else:
       raise NotImplementedError(f'Planner mode {self.mode} not recognized in planner cost set')
     self.set_cost_weights(cost_weights, constraint_cost_weights)
@@ -320,7 +317,7 @@ class LongitudinalMpc:
 
     # Update in ACC mode or ACC/e2e blend
     if self.mode == 'acc':
-      self.params[:,0] = MIN_ACCEL if self.status else self.cruise_min_a
+      self.params[:,0] = MIN_ACCEL
       self.params[:,1] = self.cruise_max_a
       self.params[:,5] = LEAD_DANGER_FACTOR
 
@@ -341,11 +338,12 @@ class LongitudinalMpc:
     elif self.mode == 'blended':
       self.params[:,0] = MIN_ACCEL
       self.params[:,1] = MAX_ACCEL
+
       self.params[:,5] = 1.0
 
       x_obstacles = np.column_stack([lead_0_obstacle,
                                      lead_1_obstacle])
-      cruise_target = T_IDXS * v_cruise + x[0]
+      cruise_target = T_IDXS * np.clip(v_cruise, v_ego - 2.0, 1e3) + x[0]
       xforward = ((v[1:] + v[:-1]) / 2) * (T_IDXS[1:] - T_IDXS[:-1])
       x = np.cumsum(np.insert(xforward, 0, x[0]))
 
@@ -385,29 +383,6 @@ class LongitudinalMpc:
          (lead_1_obstacle[0] - lead_0_obstacle[0]):
         self.source = 'lead1'
 
-      
-
-  def update_with_xva(self, x, v, a):
-    self.params[:,0] = -10.
-    self.params[:,1] = 10.
-    self.params[:,2] = 1e5
-    self.params[:,4] = T_FOLLOW
-    self.params[:,5] = LEAD_DANGER_FACTOR
-
-    # v, and a are in local frame, but x is wrt the x[0] position
-    # In >90degree turns, x goes to 0 (and may even be -ve)
-    # So, we use integral(v) + x[0] to obtain the forward-distance
-    xforward = ((v[1:] + v[:-1]) / 2) * (T_IDXS[1:] - T_IDXS[:-1])
-    x = np.cumsum(np.insert(xforward, 0, x[0]))
-    self.yref[:,1] = x
-    self.yref[:,2] = v
-    self.yref[:,3] = a
-    for i in range(N):
-      self.solver.cost_set(i, "yref", self.yref[i])
-    self.solver.cost_set(N, "yref", self.yref[N][:COST_E_DIM])
-    self.params[:,3] = np.copy(self.prev_a)
-    self.run()
-
   def run(self):
     # t0 = sec_since_boot()
     # reset = 0

selfdrive/controls/lib/longitudinal_planner.py

@@ -1,7 +1,7 @@
 #!/usr/bin/env python3
 import math
 import numpy as np
-from common.numpy_fast import interp
+from common.numpy_fast import clip, interp
 
 import cereal.messaging as messaging
 from common.conversions import Conversions as CV
@@ -18,8 +18,8 @@ from system.swaglog import cloudlog
 LON_MPC_STEP = 0.2  # first step is 0.2s
 AWARENESS_DECEL = -0.2  # car smoothly decel at .2m/s^2 when user is distracted
 A_CRUISE_MIN = -1.2
-A_CRUISE_MAX_VALS = [1.2, 1.2, 0.8, 0.6]
+A_CRUISE_MAX_VALS = [1.6, 1.2, 0.8, 0.6]
-A_CRUISE_MAX_BP = [0., 15., 25., 40.]
+A_CRUISE_MAX_BP = [0., 10.0, 25., 40.]
 
 # Lookup table for turns
 _A_TOTAL_MAX_V = [1.7, 3.2]
@@ -58,7 +58,6 @@ class LongitudinalPlanner:
 
     self.a_desired = init_a
     self.v_desired_filter = FirstOrderFilter(init_v, 2.0, DT_MDL)
-    self.t_uniform = np.arange(0.0, T_IDXS_MPC[-1] + 0.5, 0.5)
 
     self.v_desired_trajectory = np.zeros(CONTROL_N)
     self.a_desired_trajectory = np.zeros(CONTROL_N)
@@ -76,10 +75,7 @@ class LongitudinalPlanner:
       x = np.interp(T_IDXS_MPC, T_IDXS, model_msg.position.x)
       v = np.interp(T_IDXS_MPC, T_IDXS, model_msg.velocity.x)
       a = np.interp(T_IDXS_MPC, T_IDXS, model_msg.acceleration.x)
-      # Uniform interp so gradient is less noisy
+      j = np.zeros(len(T_IDXS_MPC))
-      a_sparse = np.interp(self.t_uniform, T_IDXS, model_msg.acceleration.x)
-      j_sparse = np.gradient(a_sparse, self.t_uniform)
-      j = np.interp(T_IDXS_MPC, self.t_uniform, j_sparse)
     else:
       x = np.zeros(len(T_IDXS_MPC))
       v = np.zeros(len(T_IDXS_MPC))
@@ -87,8 +83,8 @@ class LongitudinalPlanner:
       j = np.zeros(len(T_IDXS_MPC))
     return x, v, a, j
 
-  def update(self, sm):
+  def update(self, sm, read=True):
-    if self.param_read_counter % 50 == 0:
+    if self.param_read_counter % 50 == 0 and read:
       self.read_param()
     self.param_read_counter += 1
 
@@ -106,15 +102,17 @@ class LongitudinalPlanner:
     # No change cost when user is controlling the speed, or when standstill
     prev_accel_constraint = not (reset_state or sm['carState'].standstill)
 
+    accel_limits = [A_CRUISE_MIN, get_max_accel(v_ego)]
+    accel_limits_turns = limit_accel_in_turns(v_ego, sm['carState'].steeringAngleDeg, accel_limits, self.CP)
+
     if reset_state:
       self.v_desired_filter.x = v_ego
-      self.a_desired = 0.0
+      # Clip aEgo to cruise limits to prevent large accelerations when becoming active
+      self.a_desired = clip(sm['carState'].aEgo, accel_limits[0], accel_limits[1])
 
     # Prevent divergence, smooth in current v_ego
     self.v_desired_filter.x = max(0.0, self.v_desired_filter.update(v_ego))
 
-    accel_limits = [A_CRUISE_MIN, get_max_accel(v_ego)]
-    accel_limits_turns = limit_accel_in_turns(v_ego, sm['carState'].steeringAngleDeg, accel_limits, self.CP)
     if force_slow_decel:
       # if required so, force a smooth deceleration
       accel_limits_turns[1] = min(accel_limits_turns[1], AWARENESS_DECEL)

selfdrive/controls/lib/radar_helpers.py

@@ -2,8 +2,7 @@ from common.numpy_fast import mean
 from common.kalman.simple_kalman import KF1D
 
 
-# the longer lead decels, the more likely it will keep decelerating
+# Default lead acceleration decay set to 50% at 1s
-# TODO is this a good default?
 _LEAD_ACCEL_TAU = 1.5
 
 # radar tracks

selfdrive/controls/tests/test_lateral_mpc.py

@@ -1,7 +1,8 @@
 import unittest
 import numpy as np
 from selfdrive.controls.lib.lateral_mpc_lib.lat_mpc import LateralMpc
-from selfdrive.controls.lib.drive_helpers import LAT_MPC_N, CAR_ROTATION_RADIUS
+from selfdrive.controls.lib.drive_helpers import CAR_ROTATION_RADIUS
+from selfdrive.controls.lib.lateral_mpc_lib.lat_mpc import N as LAT_MPC_N
 
 
 def run_mpc(lat_mpc=None, v_ref=30., x_init=0., y_init=0., psi_init=0., curvature_init=0.,
@@ -9,7 +10,7 @@ def run_mpc(lat_mpc=None, v_ref=30., x_init=0., y_init=0., psi_init=0., curvatur
 
   if lat_mpc is None:
     lat_mpc = LateralMpc()
-  lat_mpc.set_weights(1., 1., 1.)
+  lat_mpc.set_weights(1., .1, 0.0, .05, 800)
 
   y_pts = poly_shift * np.ones(LAT_MPC_N + 1)
   heading_pts = np.zeros(LAT_MPC_N + 1)
@@ -76,9 +77,9 @@ class TestLateralMpc(unittest.TestCase):
 
   def test_switch_convergence(self):
     lat_mpc = LateralMpc()
-    sol = run_mpc(lat_mpc=lat_mpc, poly_shift=30.0, v_ref=7.0)
+    sol = run_mpc(lat_mpc=lat_mpc, poly_shift=3.0, v_ref=7.0)
     right_psi_deg = np.degrees(sol[:,2])
-    sol = run_mpc(lat_mpc=lat_mpc, poly_shift=-30.0, v_ref=7.0)
+    sol = run_mpc(lat_mpc=lat_mpc, poly_shift=-3.0, v_ref=7.0)
     left_psi_deg = np.degrees(sol[:,2])
     np.testing.assert_almost_equal(right_psi_deg, -left_psi_deg, decimal=3)
 

selfdrive/debug/check_freq.py

@@ -2,7 +2,7 @@
 import argparse
 import numpy as np
 from collections import defaultdict, deque
-from typing import DefaultDict, Deque
+from typing import DefaultDict, Deque, MutableSequence
 
 from common.realtime import sec_since_boot
 import cereal.messaging as messaging
@@ -19,7 +19,7 @@ if __name__ == "__main__":
   socket_names = args.socket
   sockets = {}
 
-  rcv_times: DefaultDict[str, Deque[float]] = defaultdict(lambda: deque(maxlen=100))
+  rcv_times: DefaultDict[str, MutableSequence[float]] = defaultdict(lambda: deque(maxlen=100))
   valids: DefaultDict[str, Deque[bool]] = defaultdict(lambda: deque(maxlen=100))
 
   t = sec_since_boot()

selfdrive/debug/check_timings.py

@@ -3,13 +3,13 @@
 import sys
 import time
 import numpy as np
-from typing import DefaultDict, Deque
+from typing import DefaultDict, MutableSequence
 from collections import defaultdict, deque
 
 import cereal.messaging as messaging
 
 socks = {s: messaging.sub_sock(s, conflate=False) for s in sys.argv[1:]}
-ts: DefaultDict[str, Deque[float]] = defaultdict(lambda: deque(maxlen=100))
+ts: DefaultDict[str, MutableSequence[float]] = defaultdict(lambda: deque(maxlen=100))
 
 if __name__ == "__main__":
   while True:

selfdrive/debug/sensor_data_to_hist.py

@@ -7,6 +7,7 @@ get interrupts in a 2kHz rate.
 
 import argparse
 import sys
+import numpy as np
 from collections import defaultdict
 
 from tools.lib.logreader import LogReader
@@ -23,16 +24,10 @@ def parseEvents(log_reader):
   lsm_data = defaultdict(list)
 
   for m in log_reader:
-    # only sensorEvents
+    if m.which() not in ['accelerometer', 'gyroscope']:
-    if m.which() != 'sensorEvents':
       continue
 
-    for se in m.sensorEvents:
+    d = getattr(m, m.which()).to_dict()
-      # convert data to dictionary
-      d = se.to_dict()
-
-      if d["timestamp"] == 0:
-        continue # empty event?
 
     if d["source"] == SRC_BMX and "acceleration" in d:
       bmx_data["accel"].append(d["timestamp"] / 1e9)
@@ -54,11 +49,7 @@ def cleanData(data):
     return [], []
 
   data.sort()
-  prev = data[0]
+  diffs = np.diff(data)
-  diffs = []
-  for v in data[1:]:
-    diffs.append(v - prev)
-    prev = v
   return data, diffs
 
 
@@ -118,4 +109,3 @@ if __name__ == "__main__":
 
   print("check plot...")
   plt.show()
-

selfdrive/debug/vw_mqb_config.py

@@ -70,8 +70,8 @@ if __name__ == "__main__":
   coding_variant, current_coding_array, coding_byte, coding_bit = None, None, 0, 0
   coding_length = len(current_coding)
 
-  # EV_SteerAssisMQB covers the majority of MQB racks (EPS_MQB_ZFLS)
+  # EV_SteerAssisMQB/MNB cover the majority of MQB racks (EPS_MQB_ZFLS)
-  if odx_file == "EV_SteerAssisMQB\x00":
+  if odx_file in ("EV_SteerAssisMQB\x00", "EV_SteerAssisMNB\x00"):
     coding_variant = "ZF"
     coding_byte = 0
     coding_bit = 4

selfdrive/locationd/laikad.py

@@ -16,7 +16,7 @@ from common.params import Params, put_nonblocking
 from laika import AstroDog
 from laika.constants import SECS_IN_HR, SECS_IN_MIN
 from laika.downloader import DownloadFailed
-from laika.ephemeris import Ephemeris, EphemerisType, convert_ublox_ephem
+from laika.ephemeris import Ephemeris, EphemerisType, convert_ublox_ephem, parse_qcom_ephem
 from laika.gps_time import GPSTime
 from laika.helpers import ConstellationId
 from laika.raw_gnss import GNSSMeasurement, correct_measurements, process_measurements, read_raw_ublox, read_raw_qcom
@@ -61,6 +61,7 @@ class Laikad:
     self.last_pos_fix = []
     self.last_pos_residual = []
     self.last_pos_fix_t = None
+    self.gps_week = None
     self.use_qcom = use_qcom
 
   def load_cache(self):
@@ -107,11 +108,11 @@ class Laikad:
     return self.last_pos_fix
 
   def is_good_report(self, gnss_msg):
-    if gnss_msg.which == 'drMeasurementReport' and self.use_qcom:
+    if gnss_msg.which() == 'drMeasurementReport' and self.use_qcom:
       constellation_id = ConstellationId.from_qcom_source(gnss_msg.drMeasurementReport.source)
       # TODO support GLONASS
       return constellation_id in [ConstellationId.GPS, ConstellationId.SBAS]
-    elif gnss_msg.which == 'measurementReport' and not self.use_qcom:
+    elif gnss_msg.which() == 'measurementReport' and not self.use_qcom:
       return True
     else:
       return False
@@ -129,9 +130,28 @@ class Laikad:
       new_meas = read_raw_ublox(report)
     return week, tow, new_meas
 
+  def is_ephemeris(self, gnss_msg):
+    if self.use_qcom:
+      return gnss_msg.which() == 'drSvPoly'
+    else:
+      return gnss_msg.which() == 'ephemeris'
+
+  def read_ephemeris(self, gnss_msg):
+    # TODO this only works on GLONASS
+    if self.use_qcom:
+      # TODO this is not robust to gps week rollover
+      if self.gps_week is None:
+        return
+      ephem = parse_qcom_ephem(gnss_msg.drSvPoly, self.gps_week)
+    else:
+      ephem = convert_ublox_ephem(gnss_msg.ephemeris)
+    self.astro_dog.add_navs({ephem.prn: [ephem]})
+    self.cache_ephemeris(t=ephem.epoch)
+
   def process_gnss_msg(self, gnss_msg, gnss_mono_time: int, block=False):
     if self.is_good_report(gnss_msg):
       week, tow, new_meas = self.read_report(gnss_msg)
+      self.gps_week = week
 
       t = gnss_mono_time * 1e-9
       if week > 0:
@@ -172,12 +192,10 @@ class Laikad:
         "correctedMeasurements": meas_msgs
       }
       return dat
-    # TODO this only works on GLONASS, qcom needs live ephemeris parsing too
+    elif self.is_ephemeris(gnss_msg):
-    elif gnss_msg.which == 'ephemeris':
+      self.read_ephemeris(gnss_msg)
-      ephem = convert_ublox_ephem(gnss_msg.ephemeris)
+
-      self.astro_dog.add_navs({ephem.prn: [ephem]})
+    #elif gnss_msg.which() == 'ionoData':
-      self.cache_ephemeris(t=ephem.epoch)
-    #elif gnss_msg.which == 'ionoData':
     # todo add this. Needed to better correct messages offline. First fix ublox_msg.cc to sent them.
 
   def update_localizer(self, est_pos, t: float, measurements: List[GNSSMeasurement]):
@@ -265,9 +283,11 @@ def create_measurement_msg(meas: GNSSMeasurement):
   c.satVel = meas.sat_vel.tolist()
   ephem = meas.sat_ephemeris
   assert ephem is not None
+  week, time_of_week = -1, -1
   if ephem.eph_type == EphemerisType.NAV:
     source_type = EphemerisSourceType.nav
-    week, time_of_week = -1, -1
+  elif ephem.eph_type == EphemerisType.QCOM_POLY:
+    source_type = EphemerisSourceType.qcom
   else:
     assert ephem.file_epoch is not None
     week = ephem.file_epoch.week
@@ -325,10 +345,11 @@ class EphemerisSourceType(IntEnum):
   nav = 0
   nasaUltraRapid = 1
   glonassIacUltraRapid = 2
+  qcom = 3
 
 
 def main(sm=None, pm=None):
-  use_qcom = os.path.isfile("/persist/comma/use-quectel-rawgps")
+  use_qcom = not Params().get_bool("UbloxAvailable", block=True)
   if use_qcom:
     raw_gnss_socket = "qcomGnss"
   else:
@@ -348,6 +369,17 @@ def main(sm=None, pm=None):
 
     if sm.updated[raw_gnss_socket]:
       gnss_msg = sm[raw_gnss_socket]
+
+      # TODO: Understand and use remaining unknown constellations
+      if gnss_msg.which() == "drMeasurementReport":
+        if getattr(gnss_msg, gnss_msg.which()).source not in ['glonass', 'gps', 'beidou', 'sbas']:
+          continue
+
+        if getattr(gnss_msg, gnss_msg.which()).gpsWeek > np.iinfo(np.int16).max:
+          # gpsWeek 65535 is received rarely from quectel, this cannot be
+          # passed to GnssMeasurements's gpsWeek (Int16)
+          continue
+
       msg = laikad.process_gnss_msg(gnss_msg, sm.logMonoTime[raw_gnss_socket], block=replay)
       if msg is not None:
         pm.send('gnssMeasurements', msg)

selfdrive/locationd/locationd.cc

@@ -20,6 +20,11 @@ const double VALID_POS_STD = 50.0; // m
 const double MAX_RESET_TRACKER = 5.0;
 const double SANE_GPS_UNCERTAINTY = 1500.0; // m
 
+// TODO: GPS sensor time offsets are empirically calculated
+// They should be replaced with synced time from a real clock
+const double GPS_LOCATION_SENSOR_TIME_OFFSET = 0.630; // s
+const double GPS_LOCATION_EXTERNAL_SENSOR_TIME_OFFSET = 0.095; // s
+
 static VectorXd floatlist2vector(const capnp::List<float, capnp::Kind::PRIMITIVE>::Reader& floatlist) {
   VectorXd res(floatlist.size());
   for (int i = 0; i < floatlist.size(); i++) {
@@ -195,17 +200,15 @@ VectorXd Localizer::get_stdev() {
   return this->kf->get_P().diagonal().array().sqrt();
 }
 
-void Localizer::handle_sensors(double current_time, const capnp::List<cereal::SensorEventData, capnp::Kind::STRUCT>::Reader& log) {
+void Localizer::handle_sensor(double current_time, const cereal::SensorEventData::Reader& log) {
   // TODO does not yet account for double sensor readings in the log
-  for (int i = 0; i < log.size(); i++) {
-    const cereal::SensorEventData::Reader& sensor_reading = log[i];
 
   // Ignore empty readings (e.g. in case the magnetometer had no data ready)
-    if (sensor_reading.getTimestamp() == 0) {
+  if (log.getTimestamp() == 0) {
-      continue;
+    return;
   }
 
-    double sensor_time = 1e-9 * sensor_reading.getTimestamp();
+  double sensor_time = 1e-9 * log.getTimestamp();
 
   // sensor time and log time should be close
   if (std::abs(current_time - sensor_time) > 0.1) {
@@ -214,13 +217,13 @@ void Localizer::handle_sensors(double current_time, const capnp::List<cereal::Se
   }
 
   // TODO: handle messages from two IMUs at the same time
-    if (sensor_reading.getSource() == cereal::SensorEventData::SensorSource::BMX055) {
+  if (log.getSource() == cereal::SensorEventData::SensorSource::BMX055) {
-      continue;
+    return;
   }
 
   // Gyro Uncalibrated
-    if (sensor_reading.getSensor() == SENSOR_GYRO_UNCALIBRATED && sensor_reading.getType() == SENSOR_TYPE_GYROSCOPE_UNCALIBRATED) {
+  if (log.getSensor() == SENSOR_GYRO_UNCALIBRATED && log.getType() == SENSOR_TYPE_GYROSCOPE_UNCALIBRATED) {
-      auto v = sensor_reading.getGyroUncalibrated().getV();
+    auto v = log.getGyroUncalibrated().getV();
     auto meas = Vector3d(-v[2], -v[1], -v[0]);
     if (meas.norm() < ROTATION_SANITY_CHECK) {
       this->kf->predict_and_observe(sensor_time, OBSERVATION_PHONE_GYRO, { meas });
@@ -228,8 +231,8 @@ void Localizer::handle_sensors(double current_time, const capnp::List<cereal::Se
   }
 
   // Accelerometer
-    if (sensor_reading.getSensor() == SENSOR_ACCELEROMETER && sensor_reading.getType() == SENSOR_TYPE_ACCELEROMETER) {
+  if (log.getSensor() == SENSOR_ACCELEROMETER && log.getType() == SENSOR_TYPE_ACCELEROMETER) {
-      auto v = sensor_reading.getAcceleration().getV();
+    auto v = log.getAcceleration().getV();
 
     // TODO: reduce false positives and re-enable this check
     // check if device fell, estimate 10 for g
@@ -242,7 +245,6 @@ void Localizer::handle_sensors(double current_time, const capnp::List<cereal::Se
     }
   }
 }
-}
 
 void Localizer::input_fake_gps_observations(double current_time) {
   // This is done to make sure that the error estimate of the position does not blow up
@@ -260,7 +262,7 @@ void Localizer::input_fake_gps_observations(double current_time) {
   this->kf->predict_and_observe(current_time, OBSERVATION_ECEF_VEL, { ecef_vel }, { ecef_vel_R });
 }
 
-void Localizer::handle_gps(double current_time, const cereal::GpsLocationData::Reader& log) {
+void Localizer::handle_gps(double current_time, const cereal::GpsLocationData::Reader& log, const double sensor_time_offset) {
   // ignore the message if the fix is invalid
   bool gps_invalid_flag = (log.getFlags() % 2 == 0);
   bool gps_unreasonable = (Vector2d(log.getAccuracy(), log.getVerticalAccuracy()).norm() >= SANE_GPS_UNCERTAINTY);
@@ -273,8 +275,10 @@ void Localizer::handle_gps(double current_time, const cereal::GpsLocationData::R
     return;
   }
   
+  double sensor_time = current_time - sensor_time_offset;
+
   // Process message
-  this->last_gps_fix = current_time;
+  this->last_gps_fix = sensor_time;
   this->gps_mode = true;
   Geodetic geodetic = { log.getLatitude(), log.getLongitude(), log.getAltitude() };
   this->converter = std::make_unique<LocalCoord>(geodetic);
@@ -303,14 +307,14 @@ void Localizer::handle_gps(double current_time, const cereal::GpsLocationData::R
   if (ecef_vel.norm() > 5.0 && orientation_error.norm() > 1.0) {
     LOGE("Locationd vs ubloxLocation orientation difference too large, kalman reset");
     this->reset_kalman(NAN, initial_pose_ecef_quat, ecef_pos, ecef_vel, ecef_pos_R, ecef_vel_R);
-    this->kf->predict_and_observe(current_time, OBSERVATION_ECEF_ORIENTATION_FROM_GPS, { initial_pose_ecef_quat });
+    this->kf->predict_and_observe(sensor_time, OBSERVATION_ECEF_ORIENTATION_FROM_GPS, { initial_pose_ecef_quat });
   } else if (gps_est_error > 100.0) {
     LOGE("Locationd vs ubloxLocation position difference too large, kalman reset");
     this->reset_kalman(NAN, initial_pose_ecef_quat, ecef_pos, ecef_vel, ecef_pos_R, ecef_vel_R);
   }
 
-  this->kf->predict_and_observe(current_time, OBSERVATION_ECEF_POS, { ecef_pos }, { ecef_pos_R });
+  this->kf->predict_and_observe(sensor_time, OBSERVATION_ECEF_POS, { ecef_pos }, { ecef_pos_R });
-  this->kf->predict_and_observe(current_time, OBSERVATION_ECEF_VEL, { ecef_vel }, { ecef_vel_R });
+  this->kf->predict_and_observe(sensor_time, OBSERVATION_ECEF_VEL, { ecef_vel }, { ecef_vel_R });
 }
 
 void Localizer::handle_car_state(double current_time, const cereal::CarState::Reader& log) {
@@ -441,12 +445,14 @@ void Localizer::handle_msg_bytes(const char *data, const size_t size) {
 void Localizer::handle_msg(const cereal::Event::Reader& log) {
   double t = log.getLogMonoTime() * 1e-9;
   this->time_check(t);
-  if (log.isSensorEvents()) {
+  if (log.isAccelerometer()) {
-    this->handle_sensors(t, log.getSensorEvents());
+    this->handle_sensor(t, log.getAccelerometer());
+  } else if (log.isGyroscope()) {
+    this->handle_sensor(t, log.getGyroscope());
   } else if (log.isGpsLocation()) {
-    this->handle_gps(t, log.getGpsLocation());
+    this->handle_gps(t, log.getGpsLocation(), GPS_LOCATION_SENSOR_TIME_OFFSET);
   } else if (log.isGpsLocationExternal()) {
-    this->handle_gps(t, log.getGpsLocationExternal());
+    this->handle_gps(t, log.getGpsLocationExternal(), GPS_LOCATION_EXTERNAL_SENSOR_TIME_OFFSET);
   } else if (log.isCarState()) {
     this->handle_car_state(t, log.getCarState());
   } else if (log.isCameraOdometry()) {
@@ -493,12 +499,13 @@ void Localizer::determine_gps_mode(double current_time) {
 
 int Localizer::locationd_thread() {
   const char* gps_location_socket;
-  if (util::file_exists("/persist/comma/use-quectel-rawgps")) {
+  if (Params().getBool("UbloxAvailable", true)) {
-    gps_location_socket = "gpsLocation";
-  } else {
     gps_location_socket = "gpsLocationExternal";
+  } else {
+    gps_location_socket = "gpsLocation";
   }
-  const std::initializer_list<const char *> service_list = {gps_location_socket, "sensorEvents", "cameraOdometry", "liveCalibration", "carState", "carParams"};
+  const std::initializer_list<const char *> service_list = {gps_location_socket, "cameraOdometry", "liveCalibration", 
+                                                          "carState", "carParams", "accelerometer", "gyroscope"};
   PubMaster pm({"liveLocationKalman"});
 
   // TODO: remove carParams once we're always sending at 100Hz
@@ -521,11 +528,11 @@ int Localizer::locationd_thread() {
     }
 
     // 100Hz publish for notcars, 20Hz for cars
-    const char* trigger_msg = sm["carParams"].getCarParams().getNotCar() ? "sensorEvents" : "cameraOdometry";
+    const char* trigger_msg = sm["carParams"].getCarParams().getNotCar() ? "accelerometer" : "cameraOdometry";
     if (sm.updated(trigger_msg)) {
       bool inputsOK = sm.allAliveAndValid();
-      bool sensorsOK = sm.alive("sensorEvents") && sm.valid("sensorEvents");
       bool gpsOK = this->isGpsOK();
+      bool sensorsOK = sm.allAliveAndValid({"accelerometer", "gyroscope"});
 
       MessageBuilder msg_builder;
       kj::ArrayPtr<capnp::byte> bytes = this->get_message_bytes(msg_builder, inputsOK, sensorsOK, gpsOK, filterInitialized);

selfdrive/locationd/locationd.h

@@ -45,8 +45,8 @@ public:
 
   void handle_msg_bytes(const char *data, const size_t size);
   void handle_msg(const cereal::Event::Reader& log);
-  void handle_sensors(double current_time, const capnp::List<cereal::SensorEventData, capnp::Kind::STRUCT>::Reader& log);
+  void handle_sensor(double current_time, const cereal::SensorEventData::Reader& log);
-  void handle_gps(double current_time, const cereal::GpsLocationData::Reader& log);
+  void handle_gps(double current_time, const cereal::GpsLocationData::Reader& log, const double sensor_time_offset);
   void handle_car_state(double current_time, const cereal::CarState::Reader& log);
   void handle_cam_odo(double current_time, const cereal::CameraOdometry::Reader& log);
   void handle_live_calib(double current_time, const cereal::LiveCalibrationData::Reader& log);

selfdrive/locationd/models/live_kf.cc

@@ -44,7 +44,7 @@ LiveKalman::LiveKalman() {
   // init filter
   this->filter = std::make_shared<EKFSym>(this->name, get_mapmat(this->Q), get_mapvec(this->initial_x),
     get_mapmat(initial_P),  this->dim_state, this->dim_state_err, 0, 0, 0, std::vector<int>(),
-    std::vector<int>{3}, std::vector<std::string>(), 0.2);
+    std::vector<int>{3}, std::vector<std::string>(), 0.8);
 }
 
 void LiveKalman::init_state(VectorXd& state, VectorXd& covs_diag, double filter_time) {

selfdrive/locationd/test/_test_locationd_lib.py

@@ -51,23 +51,23 @@ void localizer_handle_msg_bytes(Localizer_t localizer, const char *data, size_t
 
   @unittest.skip("temporarily disabled due to false positives")
   def test_device_fell(self):
-    msg = messaging.new_message('sensorEvents', 1)
+    msg = messaging.new_message('accelerometer')
-    msg.sensorEvents[0].sensor = 1
+    msg.accelerometer.sensor = 1
-    msg.sensorEvents[0].timestamp = msg.logMonoTime
+    msg.accelerometer.timestamp = msg.logMonoTime
-    msg.sensorEvents[0].type = 1
+    msg.accelerometer.type = 1
-    msg.sensorEvents[0].init('acceleration')
+    msg.accelerometer.init('acceleration')
-    msg.sensorEvents[0].acceleration.v = [10.0, 0.0, 0.0]  # zero with gravity
+    msg.accelerometer.acceleration.v = [10.0, 0.0, 0.0]  # zero with gravity
     self.localizer_handle_msg(msg)
 
     ret = self.localizer_get_msg()
     self.assertTrue(ret.liveLocationKalman.deviceStable)
 
-    msg = messaging.new_message('sensorEvents', 1)
+    msg = messaging.new_message('accelerometer')
-    msg.sensorEvents[0].sensor = 1
+    msg.accelerometer.sensor = 1
-    msg.sensorEvents[0].timestamp = msg.logMonoTime
+    msg.accelerometer.timestamp = msg.logMonoTime
-    msg.sensorEvents[0].type = 1
+    msg.accelerometer.type = 1
-    msg.sensorEvents[0].init('acceleration')
+    msg.accelerometer.init('acceleration')
-    msg.sensorEvents[0].acceleration.v = [50.1, 0.0, 0.0]  # more than 40 m/s**2
+    msg.accelerometer.acceleration.v = [50.1, 0.0, 0.0]  # more than 40 m/s**2
     self.localizer_handle_msg(msg)
 
     ret = self.localizer_get_msg()

selfdrive/locationd/test/test_locationd.py

@@ -14,13 +14,15 @@ from selfdrive.manager.process_config import managed_processes
 
 class TestLocationdProc(unittest.TestCase):
   MAX_WAITS = 1000
-  LLD_MSGS = ['gpsLocationExternal', 'cameraOdometry', 'carState', 'sensorEvents', 'liveCalibration']
+  LLD_MSGS = ['gpsLocationExternal', 'cameraOdometry', 'carState', 'liveCalibration',
+              'accelerometer', 'gyroscope', 'magnetometer']
 
   def setUp(self):
     random.seed(123489234)
 
     self.pm = messaging.PubMaster(self.LLD_MSGS)
 
+    Params().put_bool("UbloxAvailable", True)
     managed_processes['locationd'].prepare()
     managed_processes['locationd'].start()
 
@@ -51,6 +53,7 @@ class TestLocationdProc(unittest.TestCase):
       msg.gpsLocationExternal.vNED = [0.0, 0.0, 0.0]
       msg.gpsLocationExternal.latitude = self.lat
       msg.gpsLocationExternal.longitude = self.lon
+      msg.gpsLocationExternal.unixTimestampMillis = t * 1e6
       msg.gpsLocationExternal.altitude = self.alt
     elif name == 'cameraOdometry':
       msg.cameraOdometry.rot = [0.0, 0.0, 0.0]

selfdrive/locationd/torqued.py

@@ -12,7 +12,6 @@ from common.realtime import config_realtime_process, DT_MDL
 from common.filter_simple import FirstOrderFilter
 from system.swaglog import cloudlog
 from selfdrive.controls.lib.vehicle_model import ACCELERATION_DUE_TO_GRAVITY
-from selfdrive.car.toyota.values import CAR as TOYOTA
 
 HISTORY = 5  # secs
 POINTS_PER_BUCKET = 1500
@@ -33,7 +32,7 @@ MAX_INVALID_THRESHOLD = 10
 MIN_ENGAGE_BUFFER = 2  # secs
 
 VERSION = 1  # bump this to invalidate old parameter caches
-ALLOWED_FINGERPRINTS = [TOYOTA.COROLLA_TSS2, TOYOTA.COROLLA, TOYOTA.COROLLAH_TSS2]
+ALLOWED_CARS = ['toyota', 'hyundai']
 
 
 def slope2rot(slope):
@@ -42,7 +41,7 @@ def slope2rot(slope):
   return np.array([[cos, -sin], [sin, cos]])
 
 
-class npqueue:
+class NPQueue:
   def __init__(self, maxlen, rowsize):
     self.maxlen = maxlen
     self.arr = np.empty((0, rowsize))
@@ -61,7 +60,7 @@ class npqueue:
 class PointBuckets:
   def __init__(self, x_bounds, min_points):
     self.x_bounds = x_bounds
-    self.buckets = {bounds: npqueue(maxlen=POINTS_PER_BUCKET, rowsize=3) for bounds in x_bounds}
+    self.buckets = {bounds: NPQueue(maxlen=POINTS_PER_BUCKET, rowsize=3) for bounds in x_bounds}
     self.buckets_min_points = {bounds: min_point for bounds, min_point in zip(x_bounds, min_points)}
 
   def bucket_lengths(self):
@@ -80,7 +79,7 @@ class PointBuckets:
         break
 
   def get_points(self, num_points=None):
-    points = np.concatenate([x.arr for x in self.buckets.values() if len(x) > 0])
+    points = np.vstack([x.arr for x in self.buckets.values()])
     if num_points is None:
       return points
     return points[np.random.choice(np.arange(len(points)), min(len(points), num_points), replace=False)]
@@ -98,7 +97,7 @@ class TorqueEstimator:
     self.offline_friction = 0.0
     self.offline_latAccelFactor = 0.0
     self.resets = 0.0
-    self.use_params = CP.carFingerprint in ALLOWED_FINGERPRINTS
+    self.use_params = CP.carName in ALLOWED_CARS
 
     if CP.lateralTuning.which() == 'torque':
       self.offline_friction = CP.lateralTuning.torque.friction
@@ -127,12 +126,13 @@ class TorqueEstimator:
         cache_ltp = log.Event.from_bytes(torque_cache).liveTorqueParameters
         cache_CP = car.CarParams.from_bytes(params_cache)
         if self.get_restore_key(cache_CP, cache_ltp.version) == self.get_restore_key(CP, VERSION):
+          if cache_ltp.liveValid:
             initial_params = {
               'latAccelFactor': cache_ltp.latAccelFactorFiltered,
               'latAccelOffset': cache_ltp.latAccelOffsetFiltered,
-            'frictionCoefficient': cache_ltp.frictionCoefficientFiltered,
+              'frictionCoefficient': cache_ltp.frictionCoefficientFiltered
-            'points': cache_ltp.points
             }
+          initial_params['points'] = cache_ltp.points
           self.decay = cache_ltp.decay
           self.filtered_points.load_points(initial_params['points'])
           cloudlog.info("restored torque params from cache")
@@ -224,7 +224,7 @@ class TorqueEstimator:
         self.update_params({'latAccelFactor': latAccelFactor, 'latAccelOffset': latAccelOffset, 'frictionCoefficient': friction_coeff})
         self.invalid_values_tracker = max(0.0, self.invalid_values_tracker - 0.5)
       else:
-        cloudlog.exception("live torque params are numerically unstable")
+        cloudlog.exception("Live torque parameters are outside acceptable bounds.")
         liveTorqueParameters.liveValid = False
         self.invalid_values_tracker += 1.0
         # Reset when ~10 invalid over 5 secs

selfdrive/loggerd/bootlog.cc

@@ -55,13 +55,11 @@ int main(int argc, char** argv) {
   bool r = util::create_directories(LOG_ROOT + "/boot/", 0775);
   assert(r);
 
-  RawFile bz_file(path.c_str());
+  RawFile file(path.c_str());
-
   // Write initdata
-  bz_file.write(logger_build_init_data().asBytes());
+  file.write(logger_build_init_data().asBytes());
-
   // Write bootlog
-  bz_file.write(build_boot_log().asBytes());
+  file.write(build_boot_log().asBytes());
 
   return 0;
 }

selfdrive/loggerd/tests/test_loggerd.py

@@ -110,6 +110,8 @@ class TestLoggerd(unittest.TestCase):
       self.assertEqual(getattr(initData, initData_key), v)
       self.assertEqual(logged_params[param_key].decode(), v)
 
+    params.put("LaikadEphemeris", "")
+
   def test_rotation(self):
     os.environ["LOGGERD_TEST"] = "1"
     Params().put("RecordFront", "1")

selfdrive/manager/manager.py

@@ -39,6 +39,7 @@ def manager_init() -> None:
   default_params: List[Tuple[str, Union[str, bytes]]] = [
     ("CompletedTrainingVersion", "0"),
     ("DisengageOnAccelerator", "1"),
+    ("GsmMetered", "1"),
     ("HasAcceptedTerms", "0"),
     ("LanguageSetting", "main_en"),
     ("OpenpilotEnabledToggle", "1"),

selfdrive/manager/process_config.py

@@ -43,6 +43,7 @@ procs = [
   PythonProcess("deleter", "selfdrive.loggerd.deleter", offroad=True),
   PythonProcess("dmonitoringd", "selfdrive.monitoring.dmonitoringd", enabled=(not PC or WEBCAM), callback=driverview),
   PythonProcess("laikad", "selfdrive.locationd.laikad"),
+  PythonProcess("rawgpsd", "selfdrive.sensord.rawgps.rawgpsd", enabled=TICI),
   PythonProcess("navd", "selfdrive.navd.navd"),
   PythonProcess("pandad", "selfdrive.boardd.pandad", offroad=True),
   PythonProcess("paramsd", "selfdrive.locationd.paramsd"),
@@ -55,11 +56,9 @@ procs = [
   PythonProcess("uploader", "selfdrive.loggerd.uploader", offroad=True),
   PythonProcess("statsd", "selfdrive.statsd", offroad=True),
 
+  # debug procs
   NativeProcess("bridge", "cereal/messaging", ["./bridge"], onroad=False, callback=notcar),
   PythonProcess("webjoystick", "tools.joystick.web", onroad=False, callback=notcar),
-
-  # Experimental
-  PythonProcess("rawgpsd", "selfdrive.sensord.rawgps.rawgpsd", enabled=(TICI and os.path.isfile("/persist/comma/use-quectel-rawgps"))),
 ]
 
 managed_processes = {p.name: p for p in procs}