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

pull/25364/head
royjr 3 years ago
commit 4208a595da
  1. 1
      .github/ISSUE_TEMPLATE/bug_report.yml
  2. 5
      .github/ISSUE_TEMPLATE/config.yml
  3. 8
      .github/workflows/prebuilt.yaml
  4. 116
      .github/workflows/selfdrive_tests.yaml
  5. 8
      .github/workflows/setup/action.yaml
  6. 50
      .github/workflows/tools_tests.yaml
  7. 2
      .gitmodules
  8. 24
      .pre-commit-config.yaml
  9. 2
      Jenkinsfile
  10. 6
      Pipfile
  11. 1051
      Pipfile.lock
  12. 5
      RELEASES.md
  13. 7
      SConstruct
  14. 2
      cereal
  15. 2
      common/modeldata.h
  16. 13
      common/params.cc
  17. 4
      common/params.h
  18. 6
      common/params_pyx.pyx
  19. 2
      common/realtime.py
  20. 17
      common/tests/test_util.cc
  21. 16
      common/util.cc
  22. 1
      common/util.h
  23. 16
      docs/CARS.md
  24. 2
      laika_repo
  25. 2
      launch_env.sh
  26. 14
      mypy.ini
  27. 2
      opendbc
  28. 13
      release/build_devel.sh
  29. 3
      release/files_common
  30. 1
      scripts/launch_corolla.sh
  31. 16
      scripts/switch_to_master.sh
  32. 2
      selfdrive/athena/tests/helpers.py
  33. 10
      selfdrive/boardd/boardd.cc
  34. 4
      selfdrive/boardd/panda.cc
  35. 1
      selfdrive/boardd/panda.h
  36. 15
      selfdrive/car/__init__.py
  37. 9
      selfdrive/car/car_helpers.py
  38. 9
      selfdrive/car/chrysler/values.py
  39. 7
      selfdrive/car/ecu_addrs.py
  40. 50
      selfdrive/car/ford/carcontroller.py
  41. 8
      selfdrive/car/ford/carstate.py
  42. 29
      selfdrive/car/ford/fordcan.py
  43. 9
      selfdrive/car/ford/interface.py
  44. 29
      selfdrive/car/ford/values.py
  45. 4
      selfdrive/car/fw_query_definitions.py
  46. 24
      selfdrive/car/fw_versions.py
  47. 17
      selfdrive/car/gm/carcontroller.py
  48. 49
      selfdrive/car/gm/carstate.py
  49. 8
      selfdrive/car/gm/interface.py
  50. 22
      selfdrive/car/gm/values.py
  51. 4
      selfdrive/car/honda/values.py
  52. 121
      selfdrive/car/hyundai/carcontroller.py
  53. 67
      selfdrive/car/hyundai/carstate.py
  54. 21
      selfdrive/car/hyundai/hyundaican.py
  55. 118
      selfdrive/car/hyundai/hyundaicanfd.py
  56. 85
      selfdrive/car/hyundai/interface.py
  57. 78
      selfdrive/car/hyundai/values.py
  58. 14
      selfdrive/car/interfaces.py
  59. 68
      selfdrive/car/isotp_parallel_query.py
  60. 20
      selfdrive/car/mock/interface.py
  61. 5
      selfdrive/car/subaru/carstate.py
  62. 13
      selfdrive/car/subaru/values.py
  63. 46
      selfdrive/car/tesla/values.py
  64. 8
      selfdrive/car/tests/routes.py
  65. 10
      selfdrive/car/tests/test_docs.py
  66. 7
      selfdrive/car/tests/test_fw_fingerprint.py
  67. 1
      selfdrive/car/torque_data/override.yaml
  68. 4
      selfdrive/car/torque_data/substitute.yaml
  69. 16
      selfdrive/car/toyota/values.py
  70. 45
      selfdrive/car/vin.py
  71. 2
      selfdrive/car/volkswagen/carcontroller.py
  72. 11
      selfdrive/car/volkswagen/carstate.py
  73. 11
      selfdrive/car/volkswagen/interface.py
  74. 2
      selfdrive/car/volkswagen/pqcan.py
  75. 20
      selfdrive/car/volkswagen/values.py
  76. 13
      selfdrive/controls/controlsd.py
  77. 14
      selfdrive/controls/lib/drive_helpers.py
  78. 4
      selfdrive/controls/lib/events.py
  79. 14
      selfdrive/controls/lib/latcontrol_torque.py
  80. 77
      selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py
  81. 61
      selfdrive/controls/lib/lateral_planner.py
  82. 7
      selfdrive/controls/lib/longcontrol.py
  83. 35
      selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py
  84. 24
      selfdrive/controls/lib/longitudinal_planner.py
  85. 3
      selfdrive/controls/lib/radar_helpers.py
  86. 9
      selfdrive/controls/tests/test_lateral_mpc.py
  87. 4
      selfdrive/debug/check_freq.py
  88. 4
      selfdrive/debug/check_timings.py
  89. 34
      selfdrive/debug/sensor_data_to_hist.py
  90. 6
      selfdrive/debug/vw_mqb_config.py
  91. 54
      selfdrive/locationd/laikad.py
  92. 109
      selfdrive/locationd/locationd.cc
  93. 4
      selfdrive/locationd/locationd.h
  94. 2
      selfdrive/locationd/models/live_kf.cc
  95. 24
      selfdrive/locationd/test/_test_locationd_lib.py
  96. 5
      selfdrive/locationd/test/test_locationd.py
  97. 26
      selfdrive/locationd/torqued.py
  98. 8
      selfdrive/loggerd/bootlog.cc
  99. 2
      selfdrive/loggerd/tests/test_loggerd.py
  100. 1
      selfdrive/manager/manager.py
  101. Some files were not shown because too many files have changed in this diff Show More

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

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

@ -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 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 .
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 .
jobs:
build_prebuilt:
@ -37,8 +34,7 @@ jobs:
- name: Build Docker image
run: |
eval "$BUILD"
docker pull $DOCKER_REGISTRY/$IMAGE_NAME:latest || true
docker build --cache-from $DOCKER_REGISTRY/$IMAGE_NAME:latest -t $DOCKER_REGISTRY/$IMAGE_NAME:latest -f Dockerfile.openpilot .
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 .
- name: Push to container registry
run: |
$DOCKER_LOGIN

@ -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 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 .
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 .
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 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 .
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 .
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) && \
python selfdrive/test/test_valgrind_replay.py"
${{ env.RUN }} "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) && \
CI=1 coverage run selfdrive/test/process_replay/test_processes.py -j$(nproc) && \
${{ env.RUN }} "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) && \
CI=1 AZURE_TOKEN='$AZURE_TOKEN' python selfdrive/test/process_replay/test_processes.py -j$(nproc) --upload-only"
${{ env.RUN }} "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:
name: model replay onnx
test_modeld:
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) && \
ONNXCPU=1 CI=1 coverage run \
selfdrive/test/process_replay/model_replay.py -j$(nproc) && \
${{ env.RUN_CL }} "$UNIT_TEST selfdrive/modeld && \
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) && \
coverage run -m pytest selfdrive/car/tests/test_models.py && \
${{ env.RUN }} "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

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

@ -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 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 .
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 .
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 pull $DOCKER_REGISTRY/$BASE_IMAGE:latest || true
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
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 .
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
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

2
.gitmodules vendored

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

@ -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
rev: v0.931
- repo: local
hooks:
- id: mypy
exclude: '^(pyextra/)|(cereal/)|(rednose/)|(panda/)|(laika/)|(opendbc/)|(laika_repo/)|(rednose_repo/)/|(tinygrad/)|(tinygrad_repo/)'
additional_dependencies: ['types-PyYAML', 'lxml', 'numpy', 'types-atomicwrites', 'types-pycurl', 'types-requests', 'types-certifi']
args:
- --warn-redundant-casts
- --warn-return-any
- --warn-unreachable
- --warn-unused-ignores
#- --html-report=/home/batman/openpilot
name: mypy
entry: mypy
language: system
types: [python]
exclude: '^(pyextra/)|(cereal/)|(opendbc/)|(panda/)|(laika/)|(laika_repo/)|(rednose/)|(rednose_repo/)|(tinygrad/)|(tinygrad_repo/)|(xx/)'
- 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

2
Jenkinsfile vendored

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

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

1051
Pipfile.lock generated

File diff suppressed because it is too large Load Diff

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

@ -431,7 +431,12 @@ SConscript(['selfdrive/sensord/SConscript'])
SConscript(['selfdrive/ui/SConscript'])
SConscript(['selfdrive/navd/SConscript'])
SConscript(['tools/replay/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')

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

@ -4,7 +4,7 @@
#include "common/mat.h"
#include "system/hardware/hw.h"
const int TRAJECTORY_SIZE = 33;
const int TRAJECTORY_SIZE = 33;
const int LAT_MPC_N = 16;
const int LON_MPC_N = 32;
const float MIN_DRAW_DISTANCE = 10.0;

@ -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,10 +299,13 @@ std::map<std::string, std::string> Params::readAll() {
void Params::clearAll(ParamKeyType key_type) {
FileLock file_lock(params_path + "/.lock");
std::string path;
for (auto &[key, type] : keys) {
if (type & key_type) {
unlink(getParamPath(key).c_str());
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());
}
}
}

@ -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) {
return get(key) == "1";
inline bool getBool(const std::string &key, bool block = false) {
return get(key, block) == "1";
}
std::map<std::string, std::string> readAll();

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

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

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

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

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

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

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

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

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

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

@ -12,10 +12,7 @@ fi
# set git identity
source $DIR/identity.sh
echo "[-] Setting up repo T=$SECONDS"
cd $SOURCE_DIR
git fetch origin
echo "[-] Setting up target repo T=$SECONDS"
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 origin devel
git fetch --depth 1 origin master-ci
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"

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

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

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

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

@ -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) {
panda->set_safety_model(cereal::CarParams::SafetyModel::ELM327);
for (int i = 0; i < pandas.size(); i++) {
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

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

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

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

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

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

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

@ -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 = clip(apply_path_angle, -0.4995, 0.5240)
apply_angle = math.degrees(apply_path_angle) * CarControllerParams.STEER_RATIO
apply_path_angle = math.radians(apply_angle) / CarControllerParams.LKAS_STEER_RATIO
apply_path_angle = clip(apply_path_angle, -0.5, 0.5235)
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))
# if stock lane centering is active or in standby, toggle it off
can_sends.append(fordcan.create_button_command(self.packer, CS.buttons_stock_values, resume=True, bus=CANBUS.main))
# 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.STEER_RATIO
# ramp rate: 0=Slow, 1=Medium, 2=Fast, 3=Immediately
# TODO: try slower ramp speed when driver torque detected
ramp_type = 3
path_angle = math.radians(apply_angle) / CarControllerParams.LKAS_STEER_RATIO
# set slower ramp type when small steering angle change
# 0=Slow, 1=Medium, 2=Fast, 3=Immediately
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

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

@ -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
PSCM lockout.
This command can apply "Lane Keeping Aid" manoeuvres, which are subject to the 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
for traffic jam assist and highway driving. It is not subject to the PSCM
lockout.
This command can apply "Lane Centering" manoeuvres: continuous lane centering for traffic jam
assist and highway driving. It is not subject to the PSCM lockout.
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.
Ford lane centering command uses a third order polynomial to describe the road centerline. The
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
warning and traffic jam assist status.
Creates a CAN message for the Ford IPC adaptive cruise, forward collision warning and traffic jam
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.

@ -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
EventName = car.CarEvent.EventName
CarParams = car.CarParams
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]

@ -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
STEER_DRIVER_ALLOWANCE = 0.8
LKAS_STEER_RATIO = 2.75 # Approximate ratio between LatCtlPath_An_Actl and steering angle in radians
# 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),
}

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

@ -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.responseAddress = rx_addr
f.address = tx_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:
f.subAddress = addr[1]
if sub_addr is not None:
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)
print(f'TX: {hex(addr)}, RX: {hex(vin_rx_addr)}, VIN: {vin}')
vin_rx_addr, vin = get_vin(logcan, sendcan, 1, retry=10, debug=args.debug)
print(f'RX: {hex(vin_rx_addr)}, VIN: {vin}')
print(f"Getting VIN took {time.time() - t:.3f} s")
print()

@ -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:
self.lka_steering_cmd_counter_last = CS.lka_steering_cmd_counter
if CS.loopback_lka_steering_cmd_updated:
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
# 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
idx = self.lka_steering_cmd_counter % 4
can_sends.append(gmcan.create_steering_control(self.packer_pt, CanBus.POWERTRAIN, apply_steer, idx, CC.latActive))
if self.CP.openpilotLongitudinalControl:

@ -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.
ret.brake = pt_cp.vl["EBCMBrakePedalPosition"]["BrakePedalPosition"] / 0xd0
ret.brakePressed = pt_cp.vl["EBCMBrakePedalPosition"]["BrakePedalPosition"] >= 10
# Some Volt 2016-17 have loose brake pedal push rod retainers which causes the ECM to believe
# that the brake is being intermittently pressed without user interaction.
# 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"))
checks.append(("ASCMActiveCruiseControlStatus", 25))
signals += [
("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).
# While active 50 Hz (every 20 ms) is normal
# EPS will tolerate around 200ms when active before faulting
("ASCMLKASteeringCmd", 0),
]
return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, CanBus.LOOPBACK)
return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, CanBus.LOOPBACK, enforce_checks=False)

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

@ -23,11 +23,12 @@ class CarControllerParams:
ADAS_KEEPALIVE_STEP = 100
CAMERA_KEEPALIVE_STEP = 100
# Volt gasbrake lookups
# TODO: These values should be confirmed on non-Volt vehicles
# Volt gas/brake lookups
# 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_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_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", "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),
}

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

@ -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
# These cars have significantly more torque than most HKG. Limit to 70% of max.
# steering torque
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,52 +70,84 @@ 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
if self.frame % 5 == 0 and (self.CP.flags & HyundaiFlags.CANFD_HDA2):
# disable LFA on 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):
can_sends.append(hyundaicanfd.create_lfahda_cluster(self.packer, CC.enabled))
# 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))
self.last_button_frame = self.frame
else:
for _ in range(20):
can_sends.append(hyundaicanfd.create_buttons(self.packer, CS.buttons_counter+1, Buttons.CANCEL))
self.last_button_frame = self.frame
if self.frame % 5 == 0 and (not hda2 or hda2_long):
can_sends.append(hyundaicanfd.create_lfahda_cluster(self.packer, self.CP, CC.enabled))
# cruise standstill resume
elif CC.cruiseControl.resume:
if not (self.CP.flags & HyundaiFlags.CANFD_ALT_BUTTONS):
for _ in range(20):
can_sends.append(hyundaicanfd.create_buttons(self.packer, CS.buttons_counter+1, Buttons.RES_ACCEL))
self.last_button_frame = self.frame
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_acc_cancel(self.packer, self.CP, CS.cruise_info))
self.last_button_frame = self.frame
else:
for _ in range(20):
can_sends.append(hyundaicanfd.create_buttons(self.packer, CS.buttons_counter+1, Buttons.CANCEL))
self.last_button_frame = self.frame
# cruise standstill resume
elif CC.cruiseControl.resume:
if not (self.CP.flags & HyundaiFlags.CANFD_ALT_BUTTONS):
for _ in range(20):
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)
if self.CP.openpilotLongitudinalControl:
if self.frame % 100 == 0:
can_sends.append([0x7D0, 0, b"\x02\x3E\x80\x00\x00\x00\x00\x00", 0])
# Cut steer actuation bit for two frames and hold torque with induced temporary fault
torque_fault = CC.latActive and self.angle_limit_counter > MAX_ANGLE_FRAMES
lat_active = CC.latActive and not torque_fault
can_sends.append(hyundaican.create_lkas11(self.packer, self.frame, self.car_fingerprint, apply_steer, CC.latActive,
CS.lkas11, sys_warning, sys_state, CC.enabled,
hud_control.leftLaneVisible, hud_control.rightLaneVisible,
left_lane_warning, right_lane_warning))
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, lat_active,
torque_fault, CS.lkas11, sys_warning, sys_state, CC.enabled,
hud_control.leftLaneVisible, hud_control.rightLaneVisible,
left_lane_warning, right_lane_warning))
if not self.CP.openpilotLongitudinalControl:
if CC.cruiseControl.cancel:
@ -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))
self.accel = accel
hud_control.leadVisible, set_speed_in_units, stopping, CC.cruiseControl.override))
# 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

@ -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
speed_conv = CV.KPH_TO_MS if is_metric else CV.MPH_TO_MS
self.is_metric = cp.vl["CLU11"]["CF_Clu_SPEED_UNIT"] == 0
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"]:
ret.stockAeb = cp_cruise.vl["FCA11"]["FCA_CmdAct"] != 0
ret.stockFcw = cp_cruise.vl["FCA11"]["CF_VSM_Warn"] == 2
else:
ret.stockAeb = cp_cruise.vl["SCC12"]["AEB_CmdAct"] != 0
ret.stockFcw = cp_cruise.vl["SCC12"]["CF_VSM_Warn"] == 2
aeb_src = "FCA11" if self.CP.carFingerprint in FEATURES["use_fca"] else "SCC12"
aeb_sig = "FCA_CmdAct" if self.CP.carFingerprint in FEATURES["use_fca"] else "AEB_CmdAct"
aeb_warning = cp_cruise.vl[aeb_src]["CF_VSM_Warn"] != 0
aeb_braking = cp_cruise.vl[aeb_src]["CF_VSM_DecCmdAct"] != 0 or cp_cruise.vl[aeb_src][aeb_sig] != 0
ret.stockFcw = aeb_warning and not aeb_braking
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
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.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
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"),
("BYTE11", "CRUISE_INFO"),
("ZEROS_5", "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),
]

@ -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...
# (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,
"CR_FCA_Alive": 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

@ -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"
return packer.make_can_msg(msg, 4, values)
if CP.flags & HyundaiFlags.CANFD_HDA2:
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["CRUISE_STATUS"] = 0
values["CRUISE_INACTIVE"] = 1
return packer.make_can_msg("CRUISE_INFO", 4, values)
values.update({
"CRUISE_STATUS": 0,
"CRUISE_INACTIVE": 1,
})
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

@ -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,20 +284,36 @@ 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 0x50 in fingerprint[6]:
ret.flags |= HyundaiFlags.CANFD_HDA2.value
if ret.flags & HyundaiFlags.CANFD_HDA2:
ret.safetyConfigs[1].safetyParam |= Panda.FLAG_HYUNDAI_CANFD_HDA2
else:
# 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
if ret.flags & HyundaiFlags.CANFD_ALT_BUTTONS:
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)

@ -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_2019 = "KIA OPTIMA 2019"
KIA_OPTIMA_G4 = "KIA OPTIMA 4TH GEN"
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_GT_I30: None, # dashcamOnly and same platform as CAR.ELANTRA
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: [
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 2020", "All", video_link="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 2022", "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", "https://www.youtube.com/watch?v=lT7zcG6ZpGo", harness=Harness.hyundai_f),
HyundaiCarInfo("Kia Niro EV 2021", "All", "https://www.youtube.com/watch?v=lT7zcG6ZpGo", harness=Harness.hyundai_c),
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_2019: HyundaiCarInfo("Kia Optima 2019-20", harness=Harness.hyundai_g),
CAR.KIA_OPTIMA_G4: HyundaiCarInfo("Kia Optima 2017", "Advanced Smart Cruise Control", harness=Harness.hyundai_b), # TODO: may support 2016, 2018
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 ',
],
@ -1215,26 +1227,29 @@ FW_VERSIONS = {
],
},
CAR.ELANTRA_HEV_2021: {
(Ecu.fwdCamera, 0x7c4, None) : [
(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) : [
(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) :[
(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',
],
(Ecu.transmission, 0x7e1, None) :[
(Ecu.transmission, 0x7e1, None): [
b'\xf1\0006U3L0_C2\000\0006U3K3051\000\000HCN0G16NS0\xb9?A\xaa',
b'\xf1\0006U3L0_C2\000\0006U3K3051\000\000HCN0G16NS0\000\000\000\000',
b'\xf1\x816U3K3051\000\000\xf1\0006U3L0_C2\000\0006U3K3051\000\000HCN0G16NS0\xb9?A\xaa',
b'\xf1\x816U3K3051\x00\x00\xf1\x006U3L0_C2\x00\x006U3K3051\x00\x00HCN0G16NS0\x00\x00\x00\x00',
],
(Ecu.engine, 0x7e0, None) : [
(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
EV_CAR = {CAR.IONIQ_EV_2020, CAR.IONIQ_EV_LTD, CAR.KONA_EV, CAR.KIA_NIRO_EV, 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, 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, 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_2019: dbc_dict('hyundai_kia_generic', None),
CAR.KIA_OPTIMA_G4: 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

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

@ -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 = []
for a in addrs:
if isinstance(a, tuple):
self.real_addrs.append(a)
else:
self.real_addrs.append((a, None))
real_addrs = [a if isinstance(a, tuple) else (a, None) for a in addrs]
for tx_addr, _ in real_addrs:
assert tx_addr not in FUNCTIONAL_ADDRS, f"Functional address should be defined in functional_addrs: {hex(tx_addr)}"
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,13 +32,8 @@ class IsoTpParallelQuery:
for packet in can_packets:
for msg in packet.can:
if msg.src == self.bus:
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))
if msg.src == self.bus and 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):
"""Helper function to send single message"""
@ -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
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
msgs[tx_addr] = self._create_isotp_msg(*tx_addr, rx_addr)
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

@ -20,8 +20,7 @@ class CarInterface(CarInterfaceBase):
cloudlog.debug("Using Mock Car Interface")
self.sensor = messaging.sub_sock('sensorEvents')
self.gps = messaging.sub_sock('gpsLocationExternal')
self.sm = messaging.SubMaster(['gyroscope', 'gpsLocation', '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 sensors is not None:
for sensor in sensors.sensorEvents:
if sensor.type == 4: # gyro
self.yaw_rate_meas = -sensor.gyro.v[0]
gps = messaging.recv_sock(self.gps)
if gps is not None:
if self.sm.updated['gyroscope']:
self.yaw_rate_meas = -self.sm['gyroscope'].gyroUncalibrated.v[0]
gps_sock = 'gpsLocationExternal' if self.sm.rcv_frame['gpsLocationExternal'] > 1 else 'gpsLocation'
if self.sm.updated[gps_sock]:
self.prev_speed = self.speed
self.speed = gps.gpsLocationExternal.speed
self.speed = self.sm[gps_sock].speed
# create message
ret = car.CarState.new_message()

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

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

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

@ -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("018654717bc93d7d|2022-09-19--23-11-10", HYUNDAI.KIA_OPTIMA_2019, segment=0),
CarTestRoute("7653b2bce7bcfdaa|2020-03-04--15-34-32", HYUNDAI.KIA_OPTIMA_G4),
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),

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

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

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

@ -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 2019: HYUNDAI SONATA 2020
KIA OPTIMA 4TH GEN: 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

@ -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_RXH: ToyotaCarInfo("Lexus RX Hybrid 2016-19", footnotes=[Footnote.DSU]),
CAR.LEXUS_RX: [
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',

@ -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)
for (addr, rx_addr), vin in query.get_data(timeout).items():
query = IsoTpParallelQuery(sendcan, logcan, bus, addrs, [request, ], [response, ], functional_addrs=FUNCTIONAL_ADDRS, debug=debug)
results = query.get_data(timeout)
# Honda Bosch response starts with a length, trim to correct length
if vin.startswith(b'\x11'):
vin = vin[1:18]
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)
return addr[0], rx_addr, vin.decode()
print(f"vin query retry ({i+1}) ...")
# Honda Bosch response starts with a length, trim to correct length
if vin.startswith(b'\x11'):
vin = vin[1:18]
return get_rx_addr_for_tx_addr(addr), vin.decode()
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}')

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

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

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

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

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

@ -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:
@ -595,7 +601,7 @@ class Controls:
CC.enabled = self.enabled
# Check which actuators can be enabled
CC.latActive = self.active and not CS.steerFaultTemporary and not CS.steerFaultPermanent and \
CS.vEgo > self.CP.minSteerSpeed and not CS.standstill
CS.vEgo > self.CP.minSteerSpeed and not CS.standstill
CC.longActive = self.active and not self.events.any(ET.OVERRIDE_LONGITUDINAL) and self.CP.openpilotLongitudinalControl
actuators = CC.actuators
@ -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
@ -718,7 +725,7 @@ class Controls:
recent_blinker = (self.sm.frame - self.last_blinker_frame) * DT_CTRL < 5.0 # 5s blinker cooldown
ldw_allowed = self.is_ldw_enabled and CS.vEgo > LDW_MIN_SPEED and not recent_blinker \
and not CC.latActive and self.sm['liveCalibration'].calStatus == Calibration.CALIBRATED
and not CC.latActive and self.sm['liveCalibration'].calStatus == Calibration.CALIBRATED
model_v2 = self.sm['modelV2']
desire_prediction = model_v2.meta.desirePrediction

@ -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
@ -125,10 +119,10 @@ def get_lag_adjusted_curvature(CP, v_ego, psis, curvatures, curvature_rates):
desired_curvature_rate = curvature_rates[0]
max_curvature_rate = MAX_LATERAL_JERK / (v_ego**2) # inexact calculation, check https://github.com/commaai/openpilot/pull/24755
safe_desired_curvature_rate = clip(desired_curvature_rate,
-max_curvature_rate,
max_curvature_rate)
-max_curvature_rate,
max_curvature_rate)
safe_desired_curvature = clip(desired_curvature,
current_curvature_desired - max_curvature_rate * DT_MDL,
current_curvature_desired + max_curvature_rate * DT_MDL)
current_curvature_desired - max_curvature_rate * DT_MDL,
current_curvature_desired + max_curvature_rate * DT_MDL)
return safe_desired_curvature, safe_desired_curvature_rate

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

@ -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
measurement = actual_lateral_accel + low_speed_factor * actual_curvature
setpoint = desired_lateral_accel + LOW_SPEED_FACTOR * desired_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)
ff = self.torque_from_lateral_accel(gravity_adjusted_lateral_accel, self.torque_params, error, lateral_accel_deadzone, friction_compensation=True)
pid_log.error = self.torque_from_lateral_accel(error, self.torque_params, error,
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,

@ -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')
model.x = vertcat(x_ego, y_ego, psi_ego, curv_ego)
psi_rate_ego = SX.sym('psi_rate_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')
model.u = vertcat(curv_rate)
psi_accel_ego = SX.sym('psi_accel_ego')
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),
v_ego * sin(psi_ego) + rotation_radius * cos(psi_ego) * (v_ego * curv_ego),
v_ego * curv_ego,
curv_rate)
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) * psi_rate_ego,
psi_rate_ego,
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])
QR = np.diag([0.0, 0.0, 0.0])
Q = np.diag(np.zeros(COST_E_DIM))
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]
curv_rate = ocp.model.u[0]
y_ego, psi_ego, psi_rate_ego = ocp.model.x[1], ocp.model.x[2], ocp.model.x[3]
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_e = np.zeros((2, ))
# TODO hacky weights to keep behavior the same
ocp.cost.yref = np.zeros((COST_DIM, ))
ocp.cost.yref_e = np.zeros((COST_E_DIM, ))
# 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 + 5.0) * 4.0 * curv_rate))
v_ego_offset * psi_ego,
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):
W = np.asfortranarray(np.diag([path_weight, heading_weight, steer_rate_weight]))
def set_weights(self, path_weight, heading_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', (3/20.)*W[:2,:2])
self.solver.cost_set(N, 'W', W[:COST_E_DIM,:COST_E_DIM])
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[:,2] = curv_rate_pts * (v_ego+5.0) * 4.0
self.yref[:,1] = heading_pts * (v_ego + SPEED_OFFSET)
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()

@ -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.path_xyz_stds = np.column_stack([md.position.xStd, md.position.yStd, md.position.zStd])
self.plan_yaw_rate = np.array(md.orientationRate.z)
# 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
heading_cost = interp(v_ego, [5.0, 10.0], [MPC_COST_LAT.HEADING, 0.15])
self.lat_mpc.set_weights(MPC_COST_LAT.PATH, heading_cost, MPC_COST_LAT.STEER_RATE)
self.lat_mpc.set_weights(PATH_COST, LATERAL_MOTION_COST,
LATERAL_ACCEL_COST, LATERAL_JERK_COST,
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])
heading_pts = np.interp(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)
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(self.v_ego * self.t_idxs[:LAT_MPC_N + 1], np.linalg.norm(self.path_xyz, axis=1), self.plan_yaw)
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
lateral_factor = max(0, self.factor1 - (self.factor2 * v_ego**2))
p = np.array([v_ego, lateral_factor])
assert len(yaw_rate_pts) == LAT_MPC_N + 1
lateral_factor = max(0, self.factor1 - (self.factor2 * self.v_ego**2))
p = np.array([self.v_ego, lateral_factor])
self.lat_mpc.run(self.x0,
p,
y_pts,
heading_pts,
curv_rate_pts)
# init state for next
# mpc.u_sol is the desired curvature rate given x0 curv state.
# with x0[3] = measured_curvature, this would be the actual desired rate.
# instead, interpolate x_sol so that x0[3] is the desired curvature for lat_control.
yaw_rate_pts)
# init state for next iteration
# mpc.u_sol is the desired second derivative of psi given x0 curv state.
# 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 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

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

@ -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,17 +338,18 @@ 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]))
x_and_cruise = np.column_stack([x, cruise_target])
x = np.min(x_and_cruise, axis=1)
self.source = 'e2e' if x_and_cruise[0,0] < x_and_cruise[0,1] else 'cruise'
else:
@ -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

@ -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_BP = [0., 15., 25., 40.]
A_CRUISE_MAX_VALS = [1.6, 1.2, 0.8, 0.6]
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
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)
j = np.zeros(len(T_IDXS_MPC))
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):
if self.param_read_counter % 50 == 0:
def update(self, sm, read=True):
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)

@ -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
# TODO is this a good default?
# Default lead acceleration decay set to 50% at 1s
_LEAD_ACCEL_TAU = 1.5
# radar tracks

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

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

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

@ -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,28 +24,22 @@ def parseEvents(log_reader):
lsm_data = defaultdict(list)
for m in log_reader:
# only sensorEvents
if m.which() != 'sensorEvents':
if m.which() not in ['accelerometer', 'gyroscope']:
continue
for se in m.sensorEvents:
# convert data to dictionary
d = se.to_dict()
d = getattr(m, m.which()).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)
if d["source"] == SRC_BMX and "acceleration" in d:
bmx_data["accel"].append(d["timestamp"] / 1e9)
if d["source"] == SRC_BMX and "gyroUncalibrated" in d:
bmx_data["gyro"].append(d["timestamp"] / 1e9)
if d["source"] == SRC_BMX and "gyroUncalibrated" in d:
bmx_data["gyro"].append(d["timestamp"] / 1e9)
if d["source"] == SRC_LSM and "acceleration" in d:
lsm_data["accel"].append(d["timestamp"] / 1e9)
if d["source"] == SRC_LSM and "acceleration" in d:
lsm_data["accel"].append(d["timestamp"] / 1e9)
if d["source"] == SRC_LSM and "gyroUncalibrated" in d:
lsm_data["gyro"].append(d["timestamp"] / 1e9)
if d["source"] == SRC_LSM and "gyroUncalibrated" in d:
lsm_data["gyro"].append(d["timestamp"] / 1e9)
return bmx_data, lsm_data
@ -54,11 +49,7 @@ def cleanData(data):
return [], []
data.sort()
prev = data[0]
diffs = []
for v in data[1:]:
diffs.append(v - prev)
prev = v
diffs = np.diff(data)
return data, diffs
@ -118,4 +109,3 @@ if __name__ == "__main__":
print("check plot...")
plt.show()

@ -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)
if odx_file == "EV_SteerAssisMQB\x00":
# EV_SteerAssisMQB/MNB cover the majority of MQB racks (EPS_MQB_ZFLS)
if odx_file in ("EV_SteerAssisMQB\x00", "EV_SteerAssisMNB\x00"):
coding_variant = "ZF"
coding_byte = 0
coding_bit = 4
@ -111,7 +111,7 @@ if __name__ == "__main__":
if args.action in ["enable", "disable"]:
print("\nAttempting configuration update")
assert(coding_variant in ("ZF", "APA"))
assert(coding_variant in ("ZF", "APA"))
# ZF EPS config coding length can be anywhere from 1 to 4 bytes, but the
# bit we care about is always in the same place in the first byte
if args.action == "enable":

@ -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 gnss_msg.which == 'ephemeris':
ephem = convert_ublox_ephem(gnss_msg.ephemeris)
self.astro_dog.add_navs({ephem.prn: [ephem]})
self.cache_ephemeris(t=ephem.epoch)
#elif gnss_msg.which == 'ionoData':
elif self.is_ephemeris(gnss_msg):
self.read_ephemeris(gnss_msg)
#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)

@ -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,51 +200,48 @@ 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) {
continue;
}
// Ignore empty readings (e.g. in case the magnetometer had no data ready)
if (log.getTimestamp() == 0) {
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) {
LOGE("Sensor reading ignored, sensor timestamp more than 100ms off from log time");
return;
}
// sensor time and log time should be close
if (std::abs(current_time - sensor_time) > 0.1) {
LOGE("Sensor reading ignored, sensor timestamp more than 100ms off from log time");
return;
}
// TODO: handle messages from two IMUs at the same time
if (sensor_reading.getSource() == cereal::SensorEventData::SensorSource::BMX055) {
continue;
}
// TODO: handle messages from two IMUs at the same time
if (log.getSource() == cereal::SensorEventData::SensorSource::BMX055) {
return;
}
// Gyro Uncalibrated
if (sensor_reading.getSensor() == SENSOR_GYRO_UNCALIBRATED && sensor_reading.getType() == SENSOR_TYPE_GYROSCOPE_UNCALIBRATED) {
auto v = sensor_reading.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 });
}
// Gyro Uncalibrated
if (log.getSensor() == SENSOR_GYRO_UNCALIBRATED && log.getType() == SENSOR_TYPE_GYROSCOPE_UNCALIBRATED) {
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 });
}
}
// Accelerometer
if (sensor_reading.getSensor() == SENSOR_ACCELEROMETER && sensor_reading.getType() == SENSOR_TYPE_ACCELEROMETER) {
auto v = sensor_reading.getAcceleration().getV();
// Accelerometer
if (log.getSensor() == SENSOR_ACCELEROMETER && log.getType() == SENSOR_TYPE_ACCELEROMETER) {
auto v = log.getAcceleration().getV();
// TODO: reduce false positives and re-enable this check
// check if device fell, estimate 10 for g
// 40m/s**2 is a good filter for falling detection, no false positives in 20k minutes of driving
//this->device_fell |= (floatlist2vector(v) - Vector3d(10.0, 0.0, 0.0)).norm() > 40.0;
// TODO: reduce false positives and re-enable this check
// check if device fell, estimate 10 for g
// 40m/s**2 is a good filter for falling detection, no false positives in 20k minutes of driving
// this->device_fell |= (floatlist2vector(v) - Vector3d(10.0, 0.0, 0.0)).norm() > 40.0;
auto meas = Vector3d(-v[2], -v[1], -v[0]);
if (meas.norm() < ACCEL_SANITY_CHECK) {
this->kf->predict_and_observe(sensor_time, OBSERVATION_PHONE_ACCEL, { meas });
}
auto meas = Vector3d(-v[2], -v[1], -v[0]);
if (meas.norm() < ACCEL_SANITY_CHECK) {
this->kf->predict_and_observe(sensor_time, OBSERVATION_PHONE_ACCEL, { meas });
}
}
}
@ -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);
@ -268,13 +270,15 @@ void Localizer::handle_gps(double current_time, const cereal::GpsLocationData::R
bool gps_lat_lng_alt_insane = ((std::abs(log.getLatitude()) > 90) || (std::abs(log.getLongitude()) > 180) || (std::abs(log.getAltitude()) > ALTITUDE_SANITY_CHECK));
bool gps_vel_insane = (floatlist2vector(log.getVNED()).norm() > TRANS_SANITY_CHECK);
if (gps_invalid_flag || gps_unreasonable || gps_accuracy_insane || gps_lat_lng_alt_insane || gps_vel_insane){
if (gps_invalid_flag || gps_unreasonable || gps_accuracy_insane || gps_lat_lng_alt_insane || gps_vel_insane) {
this->determine_gps_mode(current_time);
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(current_time, OBSERVATION_ECEF_VEL, { ecef_vel }, { ecef_vel_R });
this->kf->predict_and_observe(sensor_time, OBSERVATION_ECEF_POS, { ecef_pos }, { ecef_pos_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()) {
this->handle_sensors(t, log.getSensorEvents());
if (log.isAccelerometer()) {
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")) {
gps_location_socket = "gpsLocation";
} else {
if (Params().getBool("UbloxAvailable", true)) {
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);

@ -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_gps(double current_time, const cereal::GpsLocationData::Reader& log);
void handle_sensor(double current_time, const cereal::SensorEventData::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);

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

@ -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.sensorEvents[0].sensor = 1
msg.sensorEvents[0].timestamp = msg.logMonoTime
msg.sensorEvents[0].type = 1
msg.sensorEvents[0].init('acceleration')
msg.sensorEvents[0].acceleration.v = [10.0, 0.0, 0.0] # zero with gravity
msg = messaging.new_message('accelerometer')
msg.accelerometer.sensor = 1
msg.accelerometer.timestamp = msg.logMonoTime
msg.accelerometer.type = 1
msg.accelerometer.init('acceleration')
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.sensorEvents[0].sensor = 1
msg.sensorEvents[0].timestamp = msg.logMonoTime
msg.sensorEvents[0].type = 1
msg.sensorEvents[0].init('acceleration')
msg.sensorEvents[0].acceleration.v = [50.1, 0.0, 0.0] # more than 40 m/s**2
msg = messaging.new_message('accelerometer')
msg.accelerometer.sensor = 1
msg.accelerometer.timestamp = msg.logMonoTime
msg.accelerometer.type = 1
msg.accelerometer.init('acceleration')
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()

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

@ -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):
initial_params = {
'latAccelFactor': cache_ltp.latAccelFactorFiltered,
'latAccelOffset': cache_ltp.latAccelOffsetFiltered,
'frictionCoefficient': cache_ltp.frictionCoefficientFiltered,
'points': cache_ltp.points
}
if cache_ltp.liveValid:
initial_params = {
'latAccelFactor': cache_ltp.latAccelFactorFiltered,
'latAccelOffset': cache_ltp.latAccelOffsetFiltered,
'frictionCoefficient': cache_ltp.frictionCoefficientFiltered
}
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

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

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

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

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

Loading…
Cancel
Save