pull/24762/head
ZwX1616 3 years ago
commit 743259bbb1
  1. 2
      .github/workflows/prebuilt.yaml
  2. 28
      .github/workflows/release.yaml
  3. 111
      .github/workflows/selfdrive_tests.yaml
  4. 29
      .github/workflows/tools_tests.yaml
  5. 4
      .gitignore
  6. 4
      Dockerfile.openpilot_base
  7. 37
      Jenkinsfile
  8. 2
      Pipfile
  9. 18
      Pipfile.lock
  10. 10
      RELEASES.md
  11. 13
      SConstruct
  12. 2
      cereal
  13. 39
      common/SConscript
  14. 4
      common/clutil.cc
  15. 0
      common/clutil.h
  16. 4
      common/gpio.cc
  17. 0
      common/gpio.h
  18. 8
      common/i2c.cc
  19. 0
      common/i2c.h
  20. 48
      common/markdown.py
  21. 0
      common/mat.h
  22. 2
      common/modeldata.h
  23. 17
      common/params.cc
  24. 3
      common/params.h
  25. 7
      common/params_pyx.pyx
  26. 0
      common/queue.h
  27. 4
      common/statlog.cc
  28. 0
      common/statlog.h
  29. 6
      common/string_helpers.py
  30. 6
      common/swaglog.cc
  31. 2
      common/swaglog.h
  32. 2
      common/tests/.gitignore
  33. 26
      common/tests/test_markdown.py
  34. 6
      common/tests/test_swaglog.cc
  35. 2
      common/tests/test_util.cc
  36. 47
      common/tests/test_xattr.py
  37. 0
      common/timing.h
  38. 2
      common/transformations/README.md
  39. 2
      common/util.cc
  40. 0
      common/util.h
  41. 0
      common/version.h
  42. 2
      common/visionimg.cc
  43. 0
      common/visionimg.h
  44. 6
      common/watchdog.cc
  45. 0
      common/watchdog.h
  46. 1
      docs/CARS.md
  47. 2
      docs/c_docs.rst
  48. 2
      laika_repo
  49. 2
      opendbc
  50. 2
      panda
  51. 57
      release/build_devel.sh
  52. 7
      release/build_release.sh
  53. 177
      release/files_common
  54. 10
      release/files_pc
  55. 9
      release/files_tici
  56. 1
      release/identity.sh
  57. 2
      scripts/waste.c
  58. 15
      selfdrive/boardd/boardd.cc
  59. 4
      selfdrive/boardd/main.cc
  60. 6
      selfdrive/boardd/panda.cc
  61. 6
      selfdrive/boardd/pigeon.cc
  62. BIN
      selfdrive/boardd/tests/test_boardd
  63. 37
      selfdrive/camerad/SConscript
  64. 43
      selfdrive/camerad/cameras/camera_common.cc
  65. 8
      selfdrive/camerad/cameras/camera_common.h
  66. 42
      selfdrive/camerad/cameras/camera_qcom2.cc
  67. 2
      selfdrive/camerad/cameras/camera_qcom2.h
  68. 125
      selfdrive/camerad/cameras/camera_replay.cc
  69. 25
      selfdrive/camerad/cameras/camera_replay.h
  70. 197
      selfdrive/camerad/cameras/camera_webcam.cc
  71. 28
      selfdrive/camerad/cameras/camera_webcam.h
  72. 290
      selfdrive/camerad/cameras/real_debayer.cl
  73. 651
      selfdrive/camerad/cameras/sensor_i2c.h
  74. 2
      selfdrive/camerad/imgproc/utils.h
  75. 4
      selfdrive/camerad/main.cc
  76. 38
      selfdrive/camerad/snapshot/snapshot.py
  77. 8
      selfdrive/camerad/test/ae_gray_test.cc
  78. 26
      selfdrive/camerad/test/camera_test.h
  79. 2
      selfdrive/camerad/transforms/rgb_to_yuv.h
  80. 2
      selfdrive/camerad/transforms/rgb_to_yuv_test.cc
  81. 9
      selfdrive/car/tesla/carcontroller.py
  82. 8
      selfdrive/car/tesla/carstate.py
  83. 2
      selfdrive/car/tesla/teslacan.py
  84. 3
      selfdrive/car/tests/routes.py
  85. 2
      selfdrive/car/tests/test_car_interfaces.py
  86. 5
      selfdrive/car/tests/test_docs.py
  87. 2
      selfdrive/car/tests/test_models.py
  88. 13
      selfdrive/car/toyota/interface.py
  89. 10
      selfdrive/car/toyota/tunes.py
  90. 27
      selfdrive/car/toyota/values.py
  91. 24
      selfdrive/car/volkswagen/values.py
  92. 4
      selfdrive/clocksd/clocksd.cc
  93. 35
      selfdrive/common/SConscript
  94. 26
      selfdrive/controls/controlsd.py
  95. 25
      selfdrive/controls/lib/drive_helpers.py
  96. 96
      selfdrive/controls/lib/events.py
  97. 84
      selfdrive/controls/lib/latcontrol_lqr.py
  98. 11
      selfdrive/controls/lib/latcontrol_torque.py
  99. 4
      selfdrive/controls/lib/tests/test_latcontrol.py
  100. 15
      selfdrive/controls/tests/test_alerts.py
  101. Some files were not shown because too many files have changed in this diff Show More

@ -30,7 +30,7 @@ jobs:
ref: master
wait-interval: 30
running-workflow-name: 'build prebuilt'
- uses: actions/checkout@v2
- uses: actions/checkout@v3
with:
submodules: true
- name: Build Docker image

@ -0,0 +1,28 @@
name: release
on:
schedule:
- cron: '0 * * * *'
workflow_dispatch:
jobs:
build_masterci:
name: build master-ci
runs-on: ubuntu-20.04
timeout-minutes: 60
if: github.repository == 'commaai/openpilot'
steps:
- name: Wait for green check mark
uses: lewagon/wait-on-check-action@v0.2
with:
ref: master
wait-interval: 30
running-workflow-name: 'build master-ci'
- uses: actions/checkout@v3
with:
submodules: true
fetch-depth: 0
- name: Pull LFS
run: git lfs pull
- name: Build master-ci
run: |
BRANCH=master-ci release/build_devel.sh

@ -26,10 +26,11 @@ jobs:
runs-on: ubuntu-20.04
timeout-minutes: 50
env:
STRIPPED_DIR: tmppilot
STRIPPED_DIR: /tmp/releasepilot
steps:
- uses: actions/checkout@v2
- uses: actions/checkout@v3
with:
fetch-depth: 0
submodules: true
- name: Check submodules
if: github.ref == 'refs/heads/master' && github.repository == 'commaai/openpilot'
@ -46,16 +47,10 @@ jobs:
restore-keys: |
scons-${{ hashFiles('.github/workflows/selfdrive_tests.yaml') }}-
scons-
- name: Strip non-release files
- name: Build devel
run: |
mkdir $STRIPPED_DIR
cp -pR --parents $(cat release/files_common) $STRIPPED_DIR
cp -pR --parents $(cat release/files_tici) $STRIPPED_DIR
cp -pR --parents $(cat release/files_pc) $STRIPPED_DIR
TARGET_DIR=$STRIPPED_DIR release/build_devel.sh
cp Dockerfile.openpilot_base $STRIPPED_DIR
# need this to build on x86
cp -pR --parents third_party/libyuv third_party/snpe selfdrive/modeld/runners $STRIPPED_DIR
- name: Build Docker image
run: eval "$BUILD"
- name: Build openpilot and run checks
@ -63,32 +58,43 @@ jobs:
cd $STRIPPED_DIR
${{ env.RUN }} "CI=1 python selfdrive/manager/build.py && \
python -m unittest discover selfdrive/car"
- name: Cleanup scons cache
run: |
cd $STRIPPED_DIR
${{ env.RUN }} "scons -j$(nproc) && \
rm -rf /tmp/scons_cache/* && \
scons -j$(nproc) --cache-populate"
build_all:
name: build all
runs-on: ubuntu-20.04
timeout-minutes: 50
steps:
- uses: actions/checkout@v2
- uses: actions/checkout@v3
with:
submodules: true
- name: Cache scons
id: scons-cache
# TODO: Change the version to the released version when https://github.com/actions/cache/pull/489 (or 571) is merged.
uses: actions/cache@03e00da99d75a2204924908e1cca7902cafce66b
env:
CACHE_SKIP_SAVE: ${{ github.ref != 'refs/heads/master' || github.repository != 'commaai/openpilot' }}
with:
path: /tmp/scons_cache
key: scons-${{ hashFiles('.github/workflows/selfdrive_tests.yaml') }}-${{ steps.date.outputs.time }}
restore-keys: |
scons-${{ hashFiles('.github/workflows/selfdrive_tests.yaml') }}-
scons-
- name: Build Docker image
run: eval "$BUILD"
- name: Build openpilot with all flags
run: ${{ env.RUN }} "scons -j$(nproc) --extras --test"
- name: Cleanup scons cache
run: |
${{ env.RUN }} "scons -j$(nproc) --extras --test && \
rm -rf /tmp/scons_cache/* && \
scons -j$(nproc) --cache-populate"
#build_mac:
# name: build macos
# runs-on: macos-latest
# timeout-minutes: 60
# steps:
# - uses: actions/checkout@v2
# - uses: actions/checkout@v3
# with:
# submodules: true
# - name: Determine pre-existing Homebrew packages
@ -147,9 +153,21 @@ jobs:
env:
IMAGE_NAME: openpilotwebcamci
steps:
- uses: actions/checkout@v2
- uses: actions/checkout@v3
with:
submodules: true
- name: Cache scons
id: scons-cache
# TODO: Change the version to the released version when https://github.com/actions/cache/pull/489 (or 571) is merged.
uses: actions/cache@03e00da99d75a2204924908e1cca7902cafce66b
env:
CACHE_SKIP_SAVE: true
with:
path: /tmp/scons_cache
key: scons-${{ hashFiles('.github/workflows/selfdrive_tests.yaml') }}-
restore-keys: |
scons-${{ hashFiles('.github/workflows/selfdrive_tests.yaml') }}-
scons-
- name: Build Docker image
run: |
eval "$BUILD"
@ -170,7 +188,7 @@ jobs:
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:
- uses: actions/checkout@v2
- uses: actions/checkout@v3
with:
submodules: true
- name: Build Docker image
@ -185,20 +203,20 @@ jobs:
runs-on: ubuntu-20.04
timeout-minutes: 50
steps:
- uses: actions/checkout@v2
- uses: actions/checkout@v3
with:
submodules: true
- name: Build Docker image
run: eval "$BUILD"
- name: pre-commit
run: ${{ env.RUN }} "git init && git add -A && pre-commit run --all"
run: ${{ env.RUN }} "pre-commit run --all"
valgrind:
name: valgrind
runs-on: ubuntu-20.04
timeout-minutes: 50
steps:
- uses: actions/checkout@v2
- uses: actions/checkout@v3
with:
submodules: true
- name: Cache dependencies
@ -241,7 +259,7 @@ jobs:
run: echo $TIMESTAMP
env:
TIMESTAMP: ${{ steps.date.outputs.time }}
- uses: actions/checkout@v2
- uses: actions/checkout@v3
with:
submodules: true
- name: Cache scons
@ -273,9 +291,9 @@ jobs:
$UNIT_TEST selfdrive/hardware/tici && \
$UNIT_TEST selfdrive/modeld && \
$UNIT_TEST tools/lib/tests && \
./common/tests/test_util && \
./common/tests/test_swaglog && \
./selfdrive/boardd/tests/test_boardd_usbprotocol && \
./selfdrive/common/tests/test_util && \
./selfdrive/common/tests/test_swaglog && \
./selfdrive/loggerd/tests/test_logger &&\
./selfdrive/proclogd/tests/test_proclog && \
./selfdrive/ui/replay/tests/test_replay && \
@ -289,7 +307,7 @@ jobs:
runs-on: ubuntu-20.04
timeout-minutes: 50
steps:
- uses: actions/checkout@v2
- uses: actions/checkout@v3
with:
submodules: true
- name: Cache dependencies
@ -317,13 +335,6 @@ jobs:
${{ env.RUN }} "scons -j$(nproc) && \
FILEREADER_CACHE=1 CI=1 coverage run selfdrive/test/process_replay/test_processes.py && \
coverage xml"
- 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 --upload-only"
- name: "Upload coverage to Codecov"
uses: codecov/codecov-action@v2
- name: Print diff
if: always()
run: cat selfdrive/test/process_replay/diff.txt
@ -333,28 +344,20 @@ jobs:
with:
name: process_replay_diff.txt
path: selfdrive/test/process_replay/diff.txt
#model_replay:
# name: model replay
# runs-on: ubuntu-20.04
# timeout-minutes: 50
# steps:
# - uses: actions/checkout@v2
# with:
# submodules: true
# - name: Build Docker image
# run: eval "$BUILD"
# - name: Run replay
# run: |
# ${{ env.RUN }} "scons -j$(nproc) && \
# selfdrive/test/process_replay/model_replay.py"
- 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 --upload-only"
- name: "Upload coverage to Codecov"
uses: codecov/codecov-action@v2
test_longitudinal:
name: longitudinal
runs-on: ubuntu-20.04
timeout-minutes: 50
steps:
- uses: actions/checkout@v2
- uses: actions/checkout@v3
with:
submodules: true
- name: Cache scons
@ -394,9 +397,9 @@ jobs:
strategy:
fail-fast: false
matrix:
job: [0, 1, 2, 3]
job: [0, 1, 2, 3, 4]
steps:
- uses: actions/checkout@v2
- uses: actions/checkout@v3
with:
submodules: true
- name: Cache dependencies
@ -426,7 +429,7 @@ jobs:
coverage xml && \
chmod -R 777 /tmp/comma_download_cache"
env:
NUM_JOBS: 4
NUM_JOBS: 5
JOB_ID: ${{ matrix.job }}
- name: "Upload coverage to Codecov"
uses: codecov/codecov-action@v2
@ -436,7 +439,7 @@ jobs:
runs-on: ubuntu-20.04
timeout-minutes: 50
steps:
- uses: actions/checkout@v2
- uses: actions/checkout@v3
with:
submodules: true
- name: Build docker container

@ -21,7 +21,7 @@ jobs:
runs-on: ubuntu-20.04
timeout-minutes: 30
steps:
- uses: actions/checkout@v2
- uses: actions/checkout@v3
with:
submodules: true
- name: Build Docker image
@ -42,28 +42,17 @@ jobs:
IMAGE_NAME: openpilot-sim
if: github.repository == 'commaai/openpilot'
steps:
- uses: actions/checkout@v2
- uses: actions/checkout@v3
with:
submodules: true
# HACK: cache LFS objects since they count against our quota
# https://github.com/actions/checkout/issues/165#issuecomment-657673315
- name: Create LFS file list
run: git lfs ls-files -l | cut -d' ' -f1 | sort > .lfs-assets-id
- name: Restore LFS cache
uses: actions/cache@v2
id: lfs-cache
with:
path: .git/lfs
key: ${{ runner.os }}-lfs-${{ hashFiles('.lfs-assets-id') }}
- name: Git LFS Pull
- name: Pull LFS
run: git lfs pull
- 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 tools/sim/Dockerfile.sim .
- name: Build base image
run: eval "$BUILD"
- name: Pull latest simulator image
run: eval "$BUILD"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 .
- name: Push to container registry
if: github.ref == 'refs/heads/master' && github.repository == 'commaai/openpilot'
run: |

4
.gitignore vendored

@ -56,7 +56,7 @@ selfdrive/modeld/_dmonitoringmodeld
/src/
one
body
/body/
openpilot
notebooks
xx
@ -77,7 +77,7 @@ cppcheck_report.txt
comma*.sh
selfdrive/modeld/thneed/compile
models/*.thneed
selfdrive/modeld/models/*.thneed
*.bz2

@ -4,7 +4,7 @@ ENV PYTHONUNBUFFERED 1
ENV DEBIAN_FRONTEND=noninteractive
RUN apt-get update && \
apt-get install -y --no-install-recommends sudo tzdata locales && \
apt-get install -y --no-install-recommends sudo tzdata locales ssh && \
rm -rf /var/lib/apt/lists/*
RUN sed -i -e 's/# en_US.UTF-8 UTF-8/en_US.UTF-8 UTF-8/' /etc/locale.gen && locale-gen
@ -29,5 +29,5 @@ RUN cd /tmp && \
cd /usr/lib/gcc/arm-none-eabi/9.2.1 && \
rm -rf arm/ && \
rm -rf thumb/nofp thumb/v6* thumb/v8* thumb/v7+fp thumb/v7-r+fp.sp
RUN sudo git config --global --add safe.directory /tmp/openpilot
RUN sudo git config --global --add safe.directory /tmp/openpilot

37
Jenkinsfile vendored

@ -80,34 +80,13 @@ pipeline {
stages {
stage('parallel tests') {
parallel {
/*
stage('Power Consumption Tests') {
steps {
lock(resource: "", label: "c2-zookeeper", inversePrecedence: true, variable: 'device_ip', quantity: 1) {
timeout(time: 90, unit: 'MINUTES') {
sh script: "/home/batman/tools/zookeeper/enable_and_wait.py $device_ip 120", label: "turn on device"
phone(device_ip, "git checkout", readFile("selfdrive/test/setup_device_ci.sh"),)
phone(device_ip, "build", "scons -j4 && sync")
sh script: "/home/batman/tools/zookeeper/disable.py $device_ip", label: "turn off device"
sh script: "/home/batman/tools/zookeeper/enable_and_wait.py $device_ip 120", label: "turn on device"
sh script: "/home/batman/tools/zookeeper/check_consumption.py 60 3", label: "idle power consumption after boot"
sh script: "/home/batman/tools/zookeeper/ignition.py 1", label: "go onroad"
sh script: "/home/batman/tools/zookeeper/check_consumption.py 60 10", label: "onroad power consumption"
sh script: "/home/batman/tools/zookeeper/ignition.py 0", label: "go offroad"
sh script: "/home/batman/tools/zookeeper/check_consumption.py 60 2", label: "idle power consumption offroad"
}
}
}
}
*/
stage('build') {
environment {
R3_PUSH = "${env.BRANCH_NAME == 'master' ? '1' : ' '}"
}
steps {
phone_steps("tici", [
["build master-ci", "cd $SOURCE_DIR/release && EXTRA_FILES='tools/' ./build_devel.sh"],
["build master-ci", "cd $SOURCE_DIR/release && TARGET_DIR=$TEST_DIR EXTRA_FILES='tools/' ./build_devel.sh"],
["build openpilot", "cd selfdrive/manager && ./build.py"],
["test manager", "python selfdrive/manager/test/test_manager.py"],
["onroad tests", "cd selfdrive/test/ && ./test_onroad.py"],
@ -124,6 +103,7 @@ pipeline {
["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"],
["test sensord", "python selfdrive/sensord/test/test_sensord.py"],
])
}
}
@ -149,18 +129,6 @@ pipeline {
}
}
stage('Push master-ci') {
when {
branch 'master'
}
steps {
phone_steps("tici-build", [
["push devel", "cd $SOURCE_DIR/release && PUSH='master-ci' ./build_devel.sh"],
])
}
}
}
post {
@ -175,4 +143,3 @@ pipeline {
}
}
}

@ -82,7 +82,7 @@ tqdm = "*"
urllib3 = "*"
utm = "*"
websocket_client = "*"
hatanaka = "*"
hatanaka = "==2.4"
[requires]
python_version = "3.8"

18
Pipfile.lock generated

@ -1,7 +1,7 @@
{
"_meta": {
"hash": {
"sha256": "72117588efb763049752818a64ea7b13448e2ec7e396e8dd4bd7f066622c5314"
"sha256": "56ab0eb7dd800742b25b7b68e335870b6a52ee27030cb044398e0a1b0ccdc9e6"
},
"pipfile-spec": 6,
"requires": {
@ -322,13 +322,13 @@
},
"hatanaka": {
"hashes": [
"sha256:0e095d35ed4f607eb77ae47ecb310e4c25f5a6267037b703ea258ed03e5c47da",
"sha256:84faa953b4f641a6d3cf8187f1775ba7e7f8d815f7bcd48cfb18553b766cbc41",
"sha256:ccf8be554deee2fc70be52bd2f1d3d4dd370001caa74333bf041933d69a19023",
"sha256:ce1628029c6b50c142a8fc5f15453c4cf2a3fd88a7128075415aeb5c9a2727d0"
"sha256:5a0624f6812b13abb4c996398a60338566885c1786841c4c04de9b1b91da28d2",
"sha256:8fda4aa56f27313de75a806a2f5aa83ed5bb2dc7561bebab856a774d06cf1ee7",
"sha256:c22970b99169bddaf22e5239672e856a6bc9602c435f8793d26ad49619a70a99",
"sha256:ef594d63473782fac46df5b0c92a59211a3efea1d47c1a964244a0abffc9f3f6"
],
"index": "pypi",
"version": "==2.8.0"
"version": "==2.4"
},
"hexdump": {
"hashes": [
@ -366,7 +366,7 @@
"sha256:6f62d78e2f89b4500b080fe3a81690850cd254227f27f75c3a0c491a1f351ba7",
"sha256:e8443a5e7a020e9d7f97f1d7d9cd17c88bcb3bc7e218bf9cf5095fe550be2951"
],
"markers": "python_version < '4' and python_full_version >= '3.6.1'",
"markers": "python_version < '4.0' and python_full_version >= '3.6.1'",
"version": "==5.10.1"
},
"itsdangerous": {
@ -1132,7 +1132,7 @@
"sha256:6657594ee297170d19f67d55c05852a874e7eb634f4f753dbd667855e07c1708",
"sha256:f1c24655a0da0d1b67f07e17a5e6b2a105894e6824b92096378bb3668ef02376"
],
"markers": "python_version < '3.10'",
"markers": "python_version >= '3.7'",
"version": "==4.2.0"
},
"urllib3": {
@ -2410,7 +2410,7 @@
"sha256:6657594ee297170d19f67d55c05852a874e7eb634f4f753dbd667855e07c1708",
"sha256:f1c24655a0da0d1b67f07e17a5e6b2a105894e6824b92096378bb3668ef02376"
],
"markers": "python_version < '3.10'",
"markers": "python_version >= '3.7'",
"version": "==4.2.0"
},
"urllib3": {

@ -1,17 +1,12 @@
Version 0.8.14 (2022-0X-XX)
Version 0.8.14 (2022-05-30)
========================
* New driving model
* Bigger model, using both of comma three's road-facing cameras
* Better at cut-in detection and tight turns
* New driver monitoring model
* Tweaked network structure to improve output resolution for dsp
* Tweaked network structure to improve output resolution for DSP
* Fixed bug in quantization aware training to reduce quantizing errors
* Resulted in 7x less MSE and no more random biases at runtime
* New lateral controller based on physical wheel torque model
* Much smoother control, consistent across the speed range
* Effective feedforward that uses road roll
* Simplified tuning, all car-specific parameters can be derived from data
* Initially used on TSS2 Corolla and TSS-P RAV4
* Added toggle to disable disengaging on the accelerator pedal
* comma body support
* Audi RS3 support thanks to jyoung8607!
@ -19,6 +14,7 @@ Version 0.8.14 (2022-0X-XX)
* Hyundai Tucson Diesel 2019 support thanks to sunnyhaibin!
* Toyota Alphard Hybrid 2021 support
* Toyota Avalon Hybrid 2022 support
* Toyota RAV4 2022 support
* Toyota RAV4 Hybrid 2022 support
Version 0.8.13 (2022-02-18)

@ -130,7 +130,7 @@ else:
"#third_party/libyuv/x64/lib",
"#third_party/mapbox-gl-native-qt/x86_64",
"#cereal",
"#selfdrive/common",
"#common",
"/usr/lib",
"/usr/local/lib",
]
@ -138,7 +138,7 @@ else:
rpath += [
Dir("#third_party/snpe/x86_64-linux-clang").abspath,
Dir("#cereal").abspath,
Dir("#selfdrive/common").abspath
Dir("#common").abspath
]
if GetOption('asan'):
@ -156,8 +156,8 @@ if arch != "Darwin":
ldflags += ["-Wl,--as-needed", "-Wl,--no-undefined"]
# Enable swaglog include in submodules
cflags += ['-DSWAGLOG="\\"selfdrive/common/swaglog.h\\""']
cxxflags += ['-DSWAGLOG="\\"selfdrive/common/swaglog.h\\""']
cflags += ['-DSWAGLOG="\\"common/swaglog.h\\""']
cxxflags += ['-DSWAGLOG="\\"common/swaglog.h\\""']
env = Environment(
ENV=lenv,
@ -212,7 +212,7 @@ env = Environment(
"#third_party",
"#opendbc/can",
"#selfdrive/boardd",
"#selfdrive/common",
"#common",
],
CYTHONCFILESUFFIX=".cpp",
COMPILATIONDB_USE_ABSPATH=True,
@ -330,7 +330,7 @@ if GetOption("clazy"):
Export('env', 'qt_env', 'arch', 'real_arch', 'SHARED', 'USE_WEBCAM')
SConscript(['selfdrive/common/SConscript'])
SConscript(['common/SConscript'])
Import('_common', '_gpucommon', '_gpu_libs')
if SHARED:
@ -384,7 +384,6 @@ SConscript(['opendbc/can/SConscript'])
SConscript(['third_party/SConscript'])
SConscript(['common/SConscript'])
SConscript(['common/kalman/SConscript'])
SConscript(['common/transformations/SConscript'])

@ -1 +1 @@
Subproject commit 7fc82bbc06900ff723992af4d7add10dc3afc0f5
Subproject commit d61736e301cf27d10b74beac278214f974c55fe4

@ -1,4 +1,39 @@
Import('envCython', 'common')
Import('env', 'envCython', 'arch', 'SHARED')
if SHARED:
fxn = env.SharedLibrary
else:
fxn = env.Library
common_libs = [
'params.cc',
'statlog.cc',
'swaglog.cc',
'util.cc',
'gpio.cc',
'i2c.cc',
'watchdog.cc',
]
_common = fxn('common', common_libs, LIBS="json11")
files = [
'clutil.cc',
'visionimg.cc',
]
if arch == "larch64":
_gpu_libs = ["GLESv2"]
else:
_gpu_libs = ["GL"]
_gpucommon = fxn('gpucommon', files, LIBS=_gpu_libs)
Export('_common', '_gpucommon', '_gpu_libs')
if GetOption('test'):
env.Program('tests/test_util', ['tests/test_util.cc'], LIBS=[_common])
env.Program('tests/test_swaglog', ['tests/test_swaglog.cc'], LIBS=[_common, 'json11', 'zmq', 'pthread'])
# Cython
envCython.Program('clock.so', 'clock.pyx')
envCython.Program('params_pyx.so', 'params_pyx.pyx', LIBS=envCython['LIBS'] + [common, 'zmq'])
envCython.Program('params_pyx.so', 'params_pyx.pyx', LIBS=envCython['LIBS'] + [_common, 'zmq', 'json11'])

@ -1,10 +1,10 @@
#include "selfdrive/common/clutil.h"
#include "common/clutil.h"
#include <cassert>
#include <iostream>
#include <memory>
#include "selfdrive/common/util.h"
#include "common/util.h"
namespace { // helper functions

@ -1,11 +1,11 @@
#include "selfdrive/common/gpio.h"
#include "common/gpio.h"
#include <fcntl.h>
#include <unistd.h>
#include <cstring>
#include "selfdrive/common/util.h"
#include "common/util.h"
// We assume that all pins have already been exported on boot,
// and that we have permission to write to them.

@ -1,4 +1,4 @@
#include "selfdrive/common/i2c.h"
#include "common/i2c.h"
#include <fcntl.h>
#include <sys/ioctl.h>
@ -8,9 +8,9 @@
#include <cstdio>
#include <stdexcept>
#include "selfdrive/common/util.h"
#include "selfdrive/common/swaglog.h"
#include "selfdrive/common/util.h"
#include "common/util.h"
#include "common/swaglog.h"
#include "common/util.h"
#define UNUSED(x) (void)(x)

@ -1,48 +0,0 @@
from typing import List
HTML_REPLACEMENTS = [
(r'&', r'&amp;'),
(r'"', r'&quot;'),
]
def parse_markdown(text: str, tab_length: int = 2) -> str:
lines = text.split("\n")
output: List[str] = []
list_level = 0
def end_outstanding_lists(level: int, end_level: int) -> int:
while level > end_level:
level -= 1
output.append("</ul>")
if level > 0:
output.append("</li>")
return end_level
for i, line in enumerate(lines):
if i + 1 < len(lines) and lines[i + 1].startswith("==="): # heading
output.append(f"<h1>{line}</h1>")
elif line.startswith("==="):
pass
elif line.lstrip().startswith("* "): # list
line_level = 1 + line.count(" " * tab_length, 0, line.index("*"))
if list_level >= line_level:
list_level = end_outstanding_lists(list_level, line_level)
else:
list_level += 1
if list_level > 1:
output[-1] = output[-1].replace("</li>", "")
output.append("<ul>")
output.append(f"<li>{line.replace('*', '', 1).lstrip()}</li>")
else:
list_level = end_outstanding_lists(list_level, 0)
if len(line) > 0:
output.append(line)
end_outstanding_lists(list_level, 0)
output_str = "\n".join(output) + "\n"
for (fr, to) in HTML_REPLACEMENTS:
output_str = output_str.replace(fr, to)
return output_str

@ -1,7 +1,7 @@
#pragma once
#include <array>
#include "selfdrive/common/mat.h"
#include "common/mat.h"
#include "selfdrive/hardware/hw.h"
const int TRAJECTORY_SIZE = 33;

@ -1,4 +1,4 @@
#include "selfdrive/common/params.h"
#include "common/params.h"
#include <dirent.h>
#include <sys/file.h>
@ -6,8 +6,8 @@
#include <csignal>
#include <unordered_map>
#include "selfdrive/common/swaglog.h"
#include "selfdrive/common/util.h"
#include "common/swaglog.h"
#include "common/util.h"
#include "selfdrive/hardware/hw.h"
namespace {
@ -60,9 +60,9 @@ bool create_params_path(const std::string &param_path, const std::string &key_pa
return true;
}
std::string ensure_params_path(const std::string &path = {}) {
std::string ensure_params_path(const std::string &prefix, const std::string &path = {}) {
std::string params_path = path.empty() ? Path::params() : path;
if (!create_params_path(params_path, params_path + "/d")) {
if (!create_params_path(params_path, params_path + prefix)) {
throw std::runtime_error(util::string_format("Failed to ensure params path, errno=%d", errno));
}
return params_path;
@ -180,9 +180,12 @@ std::unordered_map<std::string, uint32_t> keys = {
} // namespace
Params::Params(const std::string &path) {
static std::string default_param_path = ensure_params_path();
params_path = path.empty() ? default_param_path : ensure_params_path(path);
const char* env = std::getenv("OPENPILOT_PREFIX");
prefix = env ? "/" + std::string(env) : "/d";
std::string default_param_path = ensure_params_path(prefix);
params_path = path.empty() ? default_param_path : ensure_params_path(prefix, path);
}
bool Params::checkKey(const std::string &key) {

@ -18,7 +18,7 @@ public:
bool checkKey(const std::string &key);
ParamKeyType getKeyType(const std::string &key);
inline std::string getParamPath(const std::string &key = {}) {
return key.empty() ? params_path + "/d" : params_path + "/d/" + key;
return params_path + prefix + (key.empty() ? "" : "/" + key);
}
// Delete a value
@ -43,4 +43,5 @@ public:
private:
std::string params_path;
std::string prefix;
};

@ -4,7 +4,7 @@ from libcpp cimport bool
from libcpp.string cimport string
import threading
cdef extern from "selfdrive/common/params.h":
cdef extern from "common/params.h":
cpdef enum ParamKeyType:
PERSISTENT
CLEAR_ON_MANAGER_START
@ -20,6 +20,7 @@ cdef extern from "selfdrive/common/params.h":
int put(string, string) nogil
int putBool(string, bool) nogil
bool checkKey(string) nogil
string getParamPath(string) nogil
void clearAll(ParamKeyType)
@ -94,6 +95,10 @@ cdef class Params:
with nogil:
self.p.remove(k)
def get_param_path(self, key=""):
cdef string key_bytes = ensure_bytes(key)
return self.p.getParamPath(key_bytes).decode("utf-8")
def put_nonblocking(key, val, d=""):
def f(key, val):
params = Params(d)

@ -2,8 +2,8 @@
#define _GNU_SOURCE
#endif
#include "selfdrive/common/statlog.h"
#include "selfdrive/common/util.h"
#include "common/statlog.h"
#include "common/util.h"
#include <stdio.h>
#include <mutex>

@ -1,6 +0,0 @@
def replace_right(s, old, new, occurrence):
# replace_right('1232425', '2', ' ', 1) -> '12324 5'
# replace_right('1232425', '2', ' ', 2) -> '123 4 5'
split = s.rsplit(old, occurrence)
return new.join(split)

@ -2,7 +2,7 @@
#define _GNU_SOURCE
#endif
#include "selfdrive/common/swaglog.h"
#include "common/swaglog.h"
#include <cassert>
#include <cstring>
@ -13,8 +13,8 @@
#include <zmq.h>
#include "json11.hpp"
#include "selfdrive/common/util.h"
#include "selfdrive/common/version.h"
#include "common/util.h"
#include "common/version.h"
#include "selfdrive/hardware/hw.h"
class SwaglogState : public LogState {

@ -1,6 +1,6 @@
#pragma once
#include "selfdrive/common/timing.h"
#include "common/timing.h"
#define CLOUDLOG_DEBUG 10
#define CLOUDLOG_INFO 20

@ -0,0 +1,2 @@
test_util
test_swaglog

@ -1,26 +0,0 @@
#!/usr/bin/env python3
from markdown_it import MarkdownIt
import os
import unittest
from common.basedir import BASEDIR
from common.markdown import parse_markdown
class TestMarkdown(unittest.TestCase):
# validate that our simple markdown parser produces the same output as `markdown_it` from pip
def test_current_release_notes(self):
self.maxDiff = None
with open(os.path.join(BASEDIR, "RELEASES.md")) as f:
for r in f.read().split("\n\n"):
# No hyperlink support is ok
if '[' in r:
continue
self.assertEqual(MarkdownIt().render(r), parse_markdown(r))
if __name__ == "__main__":
unittest.main()

@ -4,9 +4,9 @@
#include "catch2/catch.hpp"
#include "json11.hpp"
#include "selfdrive/common/swaglog.h"
#include "selfdrive/common/util.h"
#include "selfdrive/common/version.h"
#include "common/swaglog.h"
#include "common/util.h"
#include "common/version.h"
#include "selfdrive/hardware/hw.h"
const char *SWAGLOG_ADDR = "ipc:///tmp/logmessage";

@ -10,7 +10,7 @@
#define CATCH_CONFIG_MAIN
#include "catch2/catch.hpp"
#include "selfdrive/common/util.h"
#include "common/util.h"
std::string random_bytes(int size) {
std::random_device rd;

@ -1,47 +0,0 @@
import os
import tempfile
import shutil
import unittest
from common.xattr import getxattr, setxattr, listxattr, removexattr
class TestParams(unittest.TestCase):
USER_TEST='user.test'
def setUp(self):
self.tmpdir = tempfile.mkdtemp()
self.tmpfn = os.path.join(self.tmpdir, 'test.txt')
open(self.tmpfn, 'w').close()
#print("using", self.tmpfn)
def tearDown(self):
shutil.rmtree(self.tmpdir)
def test_getxattr_none(self):
a = getxattr(self.tmpfn, TestParams.USER_TEST)
assert a is None
def test_listxattr_none(self):
l = listxattr(self.tmpfn)
assert l == []
def test_setxattr(self):
setxattr(self.tmpfn, TestParams.USER_TEST, b'123')
a = getxattr(self.tmpfn, TestParams.USER_TEST)
assert a == b'123'
def test_listxattr(self):
setxattr(self.tmpfn, 'user.test1', b'123')
setxattr(self.tmpfn, 'user.test2', b'123')
l = listxattr(self.tmpfn)
assert l == ['user.test1', 'user.test2']
def test_removexattr(self):
setxattr(self.tmpfn, TestParams.USER_TEST, b'123')
a = getxattr(self.tmpfn, TestParams.USER_TEST)
assert a == b'123'
removexattr(self.tmpfn, TestParams.USER_TEST)
a = getxattr(self.tmpfn, TestParams.USER_TEST)
assert a is None
if __name__ == "__main__":
unittest.main()

@ -24,7 +24,7 @@ by generating a rotation matrix and multiplying.
Orientation Conventations
Orientation Conventions
------
Quaternions, rotation matrices and euler angles are three
equivalent representations of orientation and all three are

@ -1,4 +1,4 @@
#include "selfdrive/common/util.h"
#include "common/util.h"
#include <sys/stat.h>
#include <dirent.h>

@ -1,4 +1,4 @@
#include "selfdrive/common/visionimg.h"
#include "common/visionimg.h"
#include <cassert>

@ -1,6 +1,6 @@
#include "selfdrive/common/watchdog.h"
#include "selfdrive/common/timing.h"
#include "selfdrive/common/util.h"
#include "common/watchdog.h"
#include "common/timing.h"
#include "common/util.h"
const std::string watchdog_fn_prefix = "/dev/shm/wd_"; // + <pid>

@ -134,6 +134,7 @@ How We Rate The Cars
|Toyota|Camry Hybrid 2018-20|All|<a href="##"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>[<sup>4</sup>](#footnotes)|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|Toyota|Highlander 2017-19|All|<a href="##"><img valign="top" src="assets/icon-star-half.svg" width="22" /></a>[<sup>3</sup>](#footnotes)|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|Toyota|Highlander Hybrid 2017-19|All|<a href="##"><img valign="top" src="assets/icon-star-half.svg" width="22" /></a>[<sup>3</sup>](#footnotes)|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|Toyota|RAV4 2022|All|<a href="##"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|Toyota|RAV4 Hybrid 2016-18|TSS-P|<a href="##"><img valign="top" src="assets/icon-star-half.svg" width="22" /></a>[<sup>3</sup>](#footnotes)|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|Toyota|RAV4 Hybrid 2022|All|<a href="##"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|Toyota|Sienna 2018-20|All|<a href="##"><img valign="top" src="assets/icon-star-half.svg" width="22" /></a>[<sup>3</sup>](#footnotes)|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|

@ -86,7 +86,7 @@ modeld
common
^^^^^^
.. autodoxygenindex::
:project: selfdrive_common
:project: common
sensorsd
^^^^^^^^

@ -1 +1 @@
Subproject commit be1a213a5ffa3cafe2b4f2d53f6df5d2452ad910
Subproject commit f5f76d28b4827c3fb706d542729651ceef6c06bd

@ -1 +1 @@
Subproject commit 9564b74d80525c9f289b730febbb2348c529c9cc
Subproject commit 210237fa635eeb76ad855c2031d2cad3bde3a2c0

@ -1 +1 @@
Subproject commit d5bd81e5b517c79e164d87b96355e6bc75915da0
Subproject commit 36c62afa0c170aa1b7a39bcae3316ffb844499e8

@ -1,25 +1,30 @@
#!/usr/bin/bash -e
#!/usr/bin/bash
set -ex
DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" >/dev/null && pwd)"
TARGET_DIR=/data/openpilot
SOURCE_DIR="$(git rev-parse --show-toplevel)"
SOURCE_DIR="$(git -C $DIR rev-parse --show-toplevel)"
if [ -z "$TARGET_DIR" ]; then
TARGET_DIR="$(mktemp -d)"
fi
# set git identity
source $DIR/identity.sh
echo "[-] Setting up repo T=$SECONDS"
if [ ! -d "$TARGET_DIR" ]; then
mkdir -p $TARGET_DIR
cd $TARGET_DIR
git init
git remote add origin git@github.com:commaai/openpilot.git
fi
cd $SOURCE_DIR
git fetch origin
rm -rf $TARGET_DIR
mkdir -p $TARGET_DIR
cd $TARGET_DIR
cp -r $SOURCE_DIR/.git $TARGET_DIR
pre-commit uninstall || true
echo "[-] bringing master-ci and devel in sync T=$SECONDS"
cd $TARGET_DIR
git prune || true
git remote prune origin || true
git fetch origin master-ci
git fetch origin devel
@ -28,6 +33,7 @@ git reset --hard master-ci
git checkout master-ci
git reset --hard origin/devel
git clean -xdf
git lfs uninstall
# remove everything except .git
echo "[-] erasing old openpilot T=$SECONDS"
@ -40,31 +46,32 @@ git clean -xdf
# do the files copy
echo "[-] copying files T=$SECONDS"
cd $SOURCE_DIR
cp -pR --parents $(cat release/files_common) $TARGET_DIR/
cp -pR --parents $(cat release/files_tici) $TARGET_DIR/
cp -pR --parents $(cat release/files_*) $TARGET_DIR/
if [ ! -z "$EXTRA_FILES" ]; then
cp -pR --parents $EXTRA_FILES $TARGET_DIR/
fi
# append source commit hash and build date to version
GIT_HASH=$(git --git-dir=$SOURCE_DIR/.git rev-parse --short HEAD)
DATETIME=$(date '+%Y-%m-%dT%H:%M:%S')
VERSION=$(cat selfdrive/common/version.h | awk -F\" '{print $2}')
echo "#define COMMA_VERSION \"$VERSION-$GIT_HASH-$DATETIME\"" > selfdrive/common/version.h
# in the directory
cd $TARGET_DIR
rm -f panda/board/obj/panda.bin.signed
# include source commit hash and build date in commit
GIT_HASH=$(git --git-dir=$SOURCE_DIR/.git rev-parse HEAD)
DATETIME=$(date '+%Y-%m-%dT%H:%M:%S')
VERSION=$(cat $SOURCE_DIR/common/version.h | awk -F\" '{print $2}')
echo "[-] committing version $VERSION T=$SECONDS"
git add -f .
git status
git commit -a -m "openpilot v$VERSION release"
git commit -a -m "openpilot v$VERSION release
date: $DATETIME
master commit: $GIT_HASH
"
if [ ! -z "$PUSH" ]; then
echo "[-] Pushing to $PUSH T=$SECONDS"
git remote set-url origin git@github.com:commaai/openpilot.git
git push -f origin master-ci:$PUSH
if [ ! -z "$BRANCH" ]; then
echo "[-] Pushing to $BRANCH T=$SECONDS"
git push -f origin master-ci:$BRANCH
fi
echo "[-] done T=$SECONDS"
echo "[-] done T=$SECONDS, ready at $TARGET_DIR"

@ -19,6 +19,7 @@ fi
# set git identity
source $DIR/identity.sh
export GIT_SSH_COMMAND="ssh -i /data/gitkey"
echo "[-] Setting up repo T=$SECONDS"
rm -rf $BUILD_DIR
@ -40,8 +41,8 @@ cd $BUILD_DIR
rm -f panda/board/obj/panda.bin.signed
VERSION=$(cat selfdrive/common/version.h | awk -F[\"-] '{print $2}')
echo "#define COMMA_VERSION \"$VERSION-release\"" > selfdrive/common/version.h
VERSION=$(cat common/version.h | awk -F[\"-] '{print $2}')
echo "#define COMMA_VERSION \"$VERSION-release\"" > common/version.h
echo "[-] committing version $VERSION T=$SECONDS"
git add -f .
@ -75,7 +76,7 @@ find . -name 'moc_*' -delete
find . -name '__pycache__' -delete
rm -rf panda/board panda/certs panda/crypto
rm -rf .sconsign.dblite Jenkinsfile release/
rm models/supercombo.dlc
rm selfdrive/modeld/models/supercombo.dlc
# Move back signed panda fw
mkdir -p panda/board/obj

@ -26,10 +26,8 @@ common/ffi_wrapper.py
common/file_helpers.py
common/logging_extra.py
common/numpy_fast.py
common/markdown.py
common/params.py
common/params_pyx.pyx
common/xattr.py
common/profiler.py
common/basedir.py
common/dict_helpers.py
@ -58,9 +56,6 @@ common/transformations/transformations.pyx
common/api/__init__.py
models/supercombo.dlc
models/dmonitoring_model_q.dlc
release/*
tools/lib/*
@ -109,85 +104,20 @@ selfdrive/car/fw_versions.py
selfdrive/car/isotp_parallel_query.py
selfdrive/car/tests/__init__.py
selfdrive/car/tests/test_car_interfaces.py
selfdrive/car/chrysler/__init__.py
selfdrive/car/chrysler/carstate.py
selfdrive/car/chrysler/interface.py
selfdrive/car/chrysler/radar_interface.py
selfdrive/car/chrysler/values.py
selfdrive/car/chrysler/carcontroller.py
selfdrive/car/chrysler/chryslercan.py
selfdrive/car/honda/__init__.py
selfdrive/car/honda/carstate.py
selfdrive/car/honda/interface.py
selfdrive/car/honda/radar_interface.py
selfdrive/car/honda/values.py
selfdrive/car/honda/carcontroller.py
selfdrive/car/honda/hondacan.py
selfdrive/car/hyundai/__init__.py
selfdrive/car/hyundai/carstate.py
selfdrive/car/hyundai/interface.py
selfdrive/car/hyundai/radar_interface.py
selfdrive/car/hyundai/values.py
selfdrive/car/hyundai/carcontroller.py
selfdrive/car/hyundai/hyundaican.py
selfdrive/car/toyota/__init__.py
selfdrive/car/toyota/carstate.py
selfdrive/car/toyota/tunes.py
selfdrive/car/toyota/interface.py
selfdrive/car/toyota/radar_interface.py
selfdrive/car/toyota/values.py
selfdrive/car/toyota/carcontroller.py
selfdrive/car/toyota/toyotacan.py
selfdrive/car/nissan/__init__.py
selfdrive/car/nissan/carcontroller.py
selfdrive/car/nissan/carstate.py
selfdrive/car/nissan/interface.py
selfdrive/car/nissan/nissancan.py
selfdrive/car/nissan/radar_interface.py
selfdrive/car/nissan/values.py
selfdrive/car/volkswagen/__init__.py
selfdrive/car/volkswagen/carstate.py
selfdrive/car/volkswagen/interface.py
selfdrive/car/volkswagen/radar_interface.py
selfdrive/car/volkswagen/values.py
selfdrive/car/volkswagen/carcontroller.py
selfdrive/car/volkswagen/volkswagencan.py
selfdrive/car/gm/__init__.py
selfdrive/car/gm/carstate.py
selfdrive/car/gm/interface.py
selfdrive/car/gm/radar_interface.py
selfdrive/car/gm/values.py
selfdrive/car/gm/carcontroller.py
selfdrive/car/gm/gmcan.py
selfdrive/car/ford/__init__.py
selfdrive/car/ford/carstate.py
selfdrive/car/ford/interface.py
selfdrive/car/ford/radar_interface.py
selfdrive/car/ford/values.py
selfdrive/car/ford/carcontroller.py
selfdrive/car/ford/fordcan.py
selfdrive/car/subaru/__init__.py
selfdrive/car/subaru/carstate.py
selfdrive/car/subaru/interface.py
selfdrive/car/subaru/radar_interface.py
selfdrive/car/subaru/values.py
selfdrive/car/subaru/carcontroller.py
selfdrive/car/subaru/subarucan.py
selfdrive/car/mazda/__init__.py
selfdrive/car/mazda/carstate.py
selfdrive/car/mazda/interface.py
selfdrive/car/mazda/radar_interface.py
selfdrive/car/mazda/values.py
selfdrive/car/mazda/carcontroller.py
selfdrive/car/mazda/mazdacan.py
selfdrive/car/tesla/__init__.py
selfdrive/car/tesla/teslacan.py
selfdrive/car/tesla/carcontroller.py
selfdrive/car/tesla/radar_interface.py
selfdrive/car/tesla/values.py
selfdrive/car/tesla/carstate.py
selfdrive/car/tesla/interface.py
selfdrive/car/body/*.py
selfdrive/car/chrysler/*.py
selfdrive/car/ford/*.py
selfdrive/car/gm/*.py
selfdrive/car/honda/*.py
selfdrive/car/hyundai/*.py
selfdrive/car/mazda/*.py
selfdrive/car/mock/*.py
selfdrive/car/nissan/*.py
selfdrive/car/subaru/*.py
selfdrive/car/tesla/*.py
selfdrive/car/toyota/*.py
selfdrive/car/volkswagen/*.py
selfdrive/clocksd/.gitignore
selfdrive/clocksd/SConscript
@ -195,34 +125,34 @@ selfdrive/clocksd/clocksd.cc
selfdrive/debug/*.py
selfdrive/common/SConscript
selfdrive/common/version.h
selfdrive/common/swaglog.h
selfdrive/common/swaglog.cc
selfdrive/common/statlog.h
selfdrive/common/statlog.cc
selfdrive/common/util.cc
selfdrive/common/util.h
selfdrive/common/queue.h
selfdrive/common/clutil.cc
selfdrive/common/clutil.h
selfdrive/common/params.h
selfdrive/common/params.cc
selfdrive/common/watchdog.cc
selfdrive/common/watchdog.h
selfdrive/common/modeldata.h
selfdrive/common/mat.h
selfdrive/common/timing.h
selfdrive/common/visionimg.cc
selfdrive/common/visionimg.h
selfdrive/common/gpio.cc
selfdrive/common/gpio.h
selfdrive/common/i2c.cc
selfdrive/common/i2c.h
common/SConscript
common/version.h
common/swaglog.h
common/swaglog.cc
common/statlog.h
common/statlog.cc
common/util.cc
common/util.h
common/queue.h
common/clutil.cc
common/clutil.h
common/params.h
common/params.cc
common/watchdog.cc
common/watchdog.h
common/modeldata.h
common/mat.h
common/timing.h
common/visionimg.cc
common/visionimg.h
common/gpio.cc
common/gpio.h
common/i2c.cc
common/i2c.h
selfdrive/controls/__init__.py
@ -238,6 +168,7 @@ selfdrive/controls/lib/events.py
selfdrive/controls/lib/lane_planner.py
selfdrive/controls/lib/latcontrol_angle.py
selfdrive/controls/lib/latcontrol_indi.py
selfdrive/controls/lib/latcontrol_lqr.py
selfdrive/controls/lib/latcontrol_torque.py
selfdrive/controls/lib/latcontrol_pid.py
selfdrive/controls/lib/latcontrol.py
@ -260,8 +191,13 @@ selfdrive/hardware/base.h
selfdrive/hardware/base.py
selfdrive/hardware/hw.h
selfdrive/hardware/tici/__init__.py
selfdrive/hardware/tici/hardware.h
selfdrive/hardware/tici/hardware.py
selfdrive/hardware/tici/pins.py
selfdrive/hardware/tici/agnos.py
selfdrive/hardware/tici/agnos.json
selfdrive/hardware/tici/amplifier.py
selfdrive/hardware/tici/updater
selfdrive/hardware/tici/iwlist.py
selfdrive/hardware/pc/__init__.py
selfdrive/hardware/pc/hardware.py
@ -302,16 +238,13 @@ selfdrive/loggerd/encoder/encoder.cc
selfdrive/loggerd/encoder/encoder.h
selfdrive/loggerd/encoder/v4l_encoder.cc
selfdrive/loggerd/encoder/v4l_encoder.h
selfdrive/loggerd/encoder/video_writer.cc
selfdrive/loggerd/encoder/video_writer.h
selfdrive/loggerd/remote_encoder.cc
selfdrive/loggerd/remote_encoder.h
selfdrive/loggerd/video_writer.cc
selfdrive/loggerd/video_writer.h
selfdrive/loggerd/logger.cc
selfdrive/loggerd/logger.h
selfdrive/loggerd/loggerd.cc
selfdrive/loggerd/loggerd.h
selfdrive/loggerd/encoderd.cc
selfdrive/loggerd/main.cc
selfdrive/loggerd/bootlog.cc
selfdrive/loggerd/encoder/ffmpeg_encoder.cc
selfdrive/loggerd/encoder/ffmpeg_encoder.h
@ -368,9 +301,6 @@ selfdrive/camerad/snapshot/*
selfdrive/camerad/include/*
selfdrive/camerad/cameras/camera_common.h
selfdrive/camerad/cameras/camera_common.cc
selfdrive/camerad/cameras/camera_replay.cc
selfdrive/camerad/cameras/camera_replay.h
selfdrive/camerad/cameras/sensor_i2c.h
selfdrive/camerad/cameras/sensor2_i2c.h
selfdrive/camerad/transforms/rgb_to_yuv.cc
@ -405,6 +335,8 @@ selfdrive/modeld/models/driving.cc
selfdrive/modeld/models/driving.h
selfdrive/modeld/models/dmonitoring.cc
selfdrive/modeld/models/dmonitoring.h
selfdrive/modeld/models/supercombo.dlc
selfdrive/modeld/models/dmonitoring_model_q.dlc
selfdrive/modeld/transforms/loadyuv.cc
selfdrive/modeld/transforms/loadyuv.h
@ -524,17 +456,14 @@ opendbc/can/common.h
opendbc/can/common.pxd
opendbc/can/common_dbc.h
opendbc/can/dbc.cc
opendbc/can/dbc.py
opendbc/can/dbc_template.cc
opendbc/can/packer.cc
opendbc/can/packer.py
opendbc/can/packer_pyx.pyx
opendbc/can/parser.cc
opendbc/can/parser.py
opendbc/can/parser_pyx.pyx
opendbc/can/process_dbc.py
opendbc/can/dbc_out/.gitkeep
opendbc/can/dbc_out/.gitignore
opendbc/comma_body.dbc
opendbc/chrysler_pacifica_2017_hybrid.dbc
opendbc/chrysler_pacifica_2017_hybrid_private_fusion.dbc

@ -1,5 +1,9 @@
selfdrive/ui/replay/*
selfdrive/modeld/runners/onnx*
third_party/mapbox-gl-native-qt/x86_64/**
third_party/mapbox-gl-native-qt/x86_64/*.so
third_party/qt-plugins/x86_64/**
third_party/qt-plugins/x86_64/geoservices/*.so
third_party/libyuv/x64/**
third_party/snpe/x86_64/**
third_party/snpe/x86_64-linux-clang/**

@ -11,15 +11,6 @@ selfdrive/camerad/cameras/camera_qcom2.cc
selfdrive/camerad/cameras/camera_qcom2.h
selfdrive/camerad/cameras/real_debayer.cl
selfdrive/hardware/tici/__init__.py
selfdrive/hardware/tici/hardware.h
selfdrive/hardware/tici/hardware.py
selfdrive/hardware/tici/pins.py
selfdrive/hardware/tici/agnos.py
selfdrive/hardware/tici/agnos.json
selfdrive/hardware/tici/amplifier.py
selfdrive/hardware/tici/updater
selfdrive/ui/qt/spinner_larch64
selfdrive/ui/qt/text_larch64
selfdrive/ui/qt/maps/*.cc

@ -2,4 +2,3 @@ export GIT_COMMITTER_NAME="Vehicle Researcher"
export GIT_COMMITTER_EMAIL="user@comma.ai"
export GIT_AUTHOR_NAME="Vehicle Researcher"
export GIT_AUTHOR_EMAIL="user@comma.ai"
export GIT_SSH_COMMAND="ssh -i /data/gitkey"

@ -11,7 +11,7 @@
#include <pthread.h>
#include <arm_neon.h>
#include <sys/sysinfo.h>
#include "../selfdrive/common/timing.h"
#include "../common/timing.h"
int get_nprocs(void);
double *ttime, *oout;

@ -23,10 +23,10 @@
#include "cereal/gen/cpp/car.capnp.h"
#include "cereal/messaging/messaging.h"
#include "selfdrive/common/params.h"
#include "selfdrive/common/swaglog.h"
#include "selfdrive/common/timing.h"
#include "selfdrive/common/util.h"
#include "common/params.h"
#include "common/swaglog.h"
#include "common/timing.h"
#include "common/util.h"
#include "selfdrive/hardware/hw.h"
#include "selfdrive/boardd/pigeon.h"
@ -115,11 +115,15 @@ bool safety_setter_thread(std::vector<Panda *> pandas) {
return false;
}
// set to ELM327 for fingerprinting
pandas[0]->set_safety_model(cereal::CarParams::SafetyModel::ELM327);
for (int i = 1; i < pandas.size(); i++) {
pandas[i]->set_safety_model(cereal::CarParams::SafetyModel::SILENT);
}
Params p = Params();
// switch to SILENT when CarVin param is read
// wait for VIN to be read
while (true) {
if (do_exit || !check_all_connected(pandas) || !ignition) {
return false;
@ -135,6 +139,7 @@ bool safety_setter_thread(std::vector<Panda *> pandas) {
util::sleep_for(20);
}
// set to ELM327 for ECU knockouts
pandas[0]->set_safety_model(cereal::CarParams::SafetyModel::ELM327, 1U);
std::string params;

@ -1,8 +1,8 @@
#include <cassert>
#include "selfdrive/boardd/boardd.h"
#include "selfdrive/common/swaglog.h"
#include "selfdrive/common/util.h"
#include "common/swaglog.h"
#include "common/util.h"
#include "selfdrive/hardware/hw.h"
int main(int argc, char *argv[]) {

@ -8,9 +8,9 @@
#include "cereal/messaging/messaging.h"
#include "panda/board/dlc_to_len.h"
#include "selfdrive/common/gpio.h"
#include "selfdrive/common/swaglog.h"
#include "selfdrive/common/util.h"
#include "common/gpio.h"
#include "common/swaglog.h"
#include "common/util.h"
static int init_usb_ctx(libusb_context **context) {
assert(context != nullptr);

@ -8,9 +8,9 @@
#include <cerrno>
#include <optional>
#include "selfdrive/common/gpio.h"
#include "selfdrive/common/swaglog.h"
#include "selfdrive/common/util.h"
#include "common/gpio.h"
#include "common/swaglog.h"
#include "common/util.h"
#include "selfdrive/locationd/ublox_msg.h"
// Termios on macos doesn't define all baud rate constants

@ -2,39 +2,18 @@ Import('env', 'arch', 'cereal', 'messaging', 'common', 'gpucommon', 'visionipc',
libs = ['m', 'pthread', common, 'jpeg', 'OpenCL', 'yuv', cereal, messaging, 'zmq', 'capnp', 'kj', visionipc, gpucommon]
cameras = []
if arch == "larch64":
libs += ['atomic']
cameras = ['cameras/camera_qcom2.cc']
else:
env['CXXFLAGS'] += ["-Wno-deprecated-declarations"]
if USE_WEBCAM:
libs += ['opencv_core', 'opencv_highgui', 'opencv_imgproc', 'opencv_videoio']
cameras = ['cameras/camera_webcam.cc']
env = env.Clone()
env.Append(CXXFLAGS = '-DWEBCAM')
env.Append(CFLAGS = '-DWEBCAM')
env.Append(CPPPATH = ['/usr/include/opencv4', '/usr/local/include/opencv4'])
else:
libs += ['avutil', 'avcodec', 'avformat', 'bz2', 'ssl', 'curl', 'crypto']
# TODO: import replay_lib from root SConstruct
cameras = ['cameras/camera_replay.cc',
env.Object('camera-util', '#/selfdrive/ui/replay/util.cc'),
env.Object('camera-framereader', '#/selfdrive/ui/replay/framereader.cc'),
env.Object('camera-filereader', '#/selfdrive/ui/replay/filereader.cc')]
if arch == "Darwin":
del libs[libs.index('OpenCL')]
del libs[libs.index(gpucommon)][gpucommon.index('GL')]
env = env.Clone()
env['FRAMEWORKS'] = ['OpenCL', 'OpenGL']
env.Program('camerad', [
'main.cc',
'cameras/camera_common.cc',
'transforms/rgb_to_yuv.cc',
'imgproc/utils.cc',
cameras,
], LIBS=libs)
env.Program('camerad', [
'main.cc',
'cameras/camera_common.cc',
'transforms/rgb_to_yuv.cc',
'imgproc/utils.cc',
cameras,
], LIBS=libs)
if GetOption("test"):
env.Program('test/ae_gray_test', [

@ -11,19 +11,17 @@
#include <jpeglib.h>
#include "selfdrive/camerad/imgproc/utils.h"
#include "selfdrive/common/clutil.h"
#include "selfdrive/common/modeldata.h"
#include "selfdrive/common/swaglog.h"
#include "selfdrive/common/util.h"
#include "common/clutil.h"
#include "common/modeldata.h"
#include "common/swaglog.h"
#include "common/util.h"
#include "selfdrive/hardware/hw.h"
#if QCOM2
#ifdef QCOM2
#include "CL/cl_ext_qcom.h"
#include "selfdrive/camerad/cameras/camera_qcom2.h"
#elif WEBCAM
#include "selfdrive/camerad/cameras/camera_webcam.h"
#else
#include "selfdrive/camerad/cameras/camera_replay.h"
#include "selfdrive/camerad/test/camera_test.h"
#endif
ExitHandler do_exit;
@ -38,26 +36,23 @@ public:
"-cl-fast-relaxed-math -cl-denorms-are-zero "
"-DFRAME_WIDTH=%d -DFRAME_HEIGHT=%d -DFRAME_STRIDE=%d -DFRAME_OFFSET=%d "
"-DRGB_WIDTH=%d -DRGB_HEIGHT=%d -DRGB_STRIDE=%d "
"-DBAYER_FLIP=%d -DHDR=%d -DCAM_NUM=%d",
"-DBAYER_FLIP=%d -DHDR=%d -DCAM_NUM=%d%s",
ci->frame_width, ci->frame_height, ci->frame_stride, ci->frame_offset,
b->rgb_width, b->rgb_height, b->rgb_stride,
ci->bayer_flip, ci->hdr, s->camera_num);
ci->bayer_flip, ci->hdr, s->camera_num, s->camera_num==1 ? " -DVIGNETTING" : "");
const char *cl_file = "cameras/real_debayer.cl";
cl_program prg_debayer = cl_program_from_file(context, device_id, cl_file, args);
krnl_ = CL_CHECK_ERR(clCreateKernel(prg_debayer, "debayer10", &err));
CL_CHECK(clReleaseProgram(prg_debayer));
}
void queue(cl_command_queue q, cl_mem cam_buf_cl, cl_mem buf_cl, int width, int height, float gain, float black_level, cl_event *debayer_event) {
void queue(cl_command_queue q, cl_mem cam_buf_cl, cl_mem buf_cl, int width, int height, cl_event *debayer_event) {
CL_CHECK(clSetKernelArg(krnl_, 0, sizeof(cl_mem), &cam_buf_cl));
CL_CHECK(clSetKernelArg(krnl_, 1, sizeof(cl_mem), &buf_cl));
const size_t globalWorkSize[] = {size_t(width / 2), size_t(height / 2)};
const int debayer_local_worksize = 16;
constexpr int localMemSize = (debayer_local_worksize + 2 * (3 / 2)) * (debayer_local_worksize + 2 * (3 / 2)) * sizeof(short int);
const size_t globalWorkSize[] = {size_t(width), size_t(height)};
const size_t localWorkSize[] = {debayer_local_worksize, debayer_local_worksize};
CL_CHECK(clSetKernelArg(krnl_, 2, localMemSize, 0));
CL_CHECK(clSetKernelArg(krnl_, 3, sizeof(float), &black_level));
CL_CHECK(clEnqueueNDRangeKernel(q, krnl_, 2, NULL, globalWorkSize, localWorkSize, 0, 0, debayer_event));
}
@ -141,6 +136,7 @@ bool CameraBuf::acquire() {
cur_frame_data = camera_bufs_metadata[cur_buf_idx];
cur_rgb_buf = vipc_server->get_buffer(rgb_type);
cur_yuv_buf = vipc_server->get_buffer(yuv_type);
cl_mem camrabuf_cl = camera_bufs[cur_buf_idx].buf_cl;
cl_event event;
@ -149,26 +145,15 @@ bool CameraBuf::acquire() {
cur_camera_buf = &camera_bufs[cur_buf_idx];
if (debayer) {
float gain = 0.0;
float black_level = 42.0;
#ifndef QCOM2
gain = camera_state->digital_gain;
if ((int)gain == 0) gain = 1.0;
#else
if (camera_state->camera_id == CAMERA_ID_IMX390) black_level = 64.0;
#endif
debayer->queue(q, camrabuf_cl, cur_rgb_buf->buf_cl, rgb_width, rgb_height, gain, black_level, &event);
debayer->queue(q, camrabuf_cl, cur_yuv_buf->buf_cl, rgb_width, rgb_height, &event);
} else {
assert(rgb_stride == camera_state->ci.frame_stride);
CL_CHECK(clEnqueueCopyBuffer(q, camrabuf_cl, cur_rgb_buf->buf_cl, 0, 0, cur_rgb_buf->len, 0, 0, &event));
rgb2yuv->queue(q, camrabuf_cl, cur_rgb_buf->buf_cl);
}
clWaitForEvents(1, &event);
CL_CHECK(clReleaseEvent(event));
cur_yuv_buf = vipc_server->get_buffer(yuv_type);
rgb2yuv->queue(q, cur_rgb_buf->buf_cl, cur_yuv_buf->buf_cl);
cur_frame_data.processing_time = (millis_since_boot() - start_time) / 1000.0;
VisionIpcBufExtra extra = {
@ -176,9 +161,7 @@ bool CameraBuf::acquire() {
cur_frame_data.timestamp_sof,
cur_frame_data.timestamp_eof,
};
cur_rgb_buf->set_frame_id(cur_frame_data.frame_id);
cur_yuv_buf->set_frame_id(cur_frame_data.frame_id);
vipc_server->send(cur_rgb_buf, &extra);
vipc_server->send(cur_yuv_buf, &extra);
return true;

@ -10,10 +10,10 @@
#include "cereal/visionipc/visionipc.h"
#include "cereal/visionipc/visionipc_server.h"
#include "selfdrive/camerad/transforms/rgb_to_yuv.h"
#include "selfdrive/common/mat.h"
#include "selfdrive/common/queue.h"
#include "selfdrive/common/swaglog.h"
#include "selfdrive/common/visionimg.h"
#include "common/mat.h"
#include "common/queue.h"
#include "common/swaglog.h"
#include "common/visionimg.h"
#include "selfdrive/hardware/hw.h"
#define CAMERA_ID_IMX298 0

@ -19,7 +19,7 @@
#include "media/cam_sensor.h"
#include "media/cam_sensor_cmn_header.h"
#include "media/cam_sync.h"
#include "selfdrive/common/swaglog.h"
#include "common/swaglog.h"
#include "selfdrive/camerad/cameras/sensor2_i2c.h"
// For debugging:
@ -91,8 +91,7 @@ int do_cam_control(int fd, int op_code, void *handle, int size) {
int ret = HANDLE_EINTR(ioctl(fd, VIDIOC_CAM_CONTROL, &camcontrol));
if (ret == -1) {
printf("OP CODE ERR - %d \n", op_code);
perror("wat");
LOGE("VIDIOC_CAM_CONTROL error: op_code %d - errno %d", op_code, errno);
}
return ret;
}
@ -546,7 +545,7 @@ void CameraState::config_isp(int io_mem_handle, int fence, int request_id, int b
int ret = device_config(multi_cam_state->isp_fd, session_handle, isp_dev_handle, cam_packet_handle);
assert(ret == 0);
if (ret != 0) {
printf("ISP CONFIG FAILED\n");
LOGE("isp config failed");
}
mm.free(buf2);
@ -563,7 +562,10 @@ void CameraState::enqueue_buffer(int i, bool dp) {
sync_wait.sync_obj = sync_objs[i];
sync_wait.timeout_ms = 50; // max dt tolerance, typical should be 23
ret = do_cam_control(multi_cam_state->cam_sync_fd, CAM_SYNC_WAIT, &sync_wait, sizeof(sync_wait));
// LOGD("fence wait: %d %d", ret, sync_wait.sync_obj);
if (ret != 0) {
LOGE("failed to wait for sync: %d %d", ret, sync_wait.sync_obj);
// TODO: handle frame drop cleanly
}
buf.camera_bufs_metadata[i].timestamp_eof = (uint64_t)nanos_since_boot(); // set true eof
if (dp) buf.queue(i);
@ -572,14 +574,18 @@ void CameraState::enqueue_buffer(int i, bool dp) {
struct cam_sync_info sync_destroy = {0};
sync_destroy.sync_obj = sync_objs[i];
ret = do_cam_control(multi_cam_state->cam_sync_fd, CAM_SYNC_DESTROY, &sync_destroy, sizeof(sync_destroy));
// LOGD("fence destroy: %d %d", ret, sync_destroy.sync_obj);
if (ret != 0) {
LOGE("failed to destroy sync object: %d %d", ret, sync_destroy.sync_obj);
}
}
// create output fence
struct cam_sync_info sync_create = {0};
strcpy(sync_create.name, "NodeOutputPortFence");
ret = do_cam_control(multi_cam_state->cam_sync_fd, CAM_SYNC_CREATE, &sync_create, sizeof(sync_create));
// LOGD("fence req: %d %d", ret, sync_create.sync_obj);
if (ret != 0) {
LOGE("failed to create fence: %d %d", ret, sync_create.sync_obj)
}
sync_objs[i] = sync_create.sync_obj;
// schedule request with camera request manager
@ -588,11 +594,12 @@ void CameraState::enqueue_buffer(int i, bool dp) {
req_mgr_sched_request.link_hdl = link_handle;
req_mgr_sched_request.req_id = request_id;
ret = do_cam_control(multi_cam_state->video0_fd, CAM_REQ_MGR_SCHED_REQ, &req_mgr_sched_request, sizeof(req_mgr_sched_request));
// LOGD("sched req: %d %d", ret, request_id);
if (ret != 0) {
LOGE("failed to schedule cam mgr request: %d %d", ret, request_id);
}
// poke sensor, must happen after schedule
sensors_poke(request_id);
// LOGD("Poked sensor");
// submit request to the ife
config_isp(buf_handle[i], sync_objs[i], request_id, buf0_handle, 65632*(i+1));
@ -873,14 +880,14 @@ void cameras_open(MultiCameraState *s) {
sub.type = V4L_EVENT_CAM_REQ_MGR_EVENT;
sub.id = V4L_EVENT_CAM_REQ_MGR_SOF_BOOT_TS;
ret = HANDLE_EINTR(ioctl(s->video0_fd, VIDIOC_SUBSCRIBE_EVENT, &sub));
printf("req mgr subscribe: %d\n", ret);
LOGD("req mgr subscribe: %d", ret);
s->driver_cam.camera_open();
printf("driver camera opened \n");
LOGD("driver camera opened");
s->road_cam.camera_open();
printf("road camera opened \n");
LOGD("road camera opened");
s->wide_road_cam.camera_open();
printf("wide road camera opened \n");
LOGD("wide road camera opened");
}
void CameraState::camera_close() {
@ -1196,7 +1203,14 @@ static float ar0231_parse_temp_sensor(uint16_t calib1, uint16_t calib2, uint16_t
}
static void ar0231_process_registers(MultiCameraState *s, CameraState *c, cereal::FrameData::Builder &framed){
const uint8_t expected_preamble[] = {0x0a, 0xaa, 0x55, 0x20, 0xa5, 0x55};
uint8_t *data = (uint8_t*)c->buf.cur_camera_buf->addr + c->ci.registers_offset;
if (memcmp(data, expected_preamble, std::size(expected_preamble)) != 0){
LOGE("unexpected register data found");
return;
}
auto registers = c->ar0231_parse_registers(data, {0x2000, 0x2002, 0x20b0, 0x20b2, 0x30c6, 0x30c8, 0x30ca, 0x30cc});
uint32_t frame_id = ((uint32_t)registers[0x2000] << 16) | registers[0x2002];
@ -1306,7 +1320,7 @@ void cameras_run(MultiCameraState *s) {
} else if (event_data->session_hdl == s->driver_cam.session_handle) {
s->driver_cam.handle_camera_event(event_data);
} else {
printf("Unknown vidioc event source\n");
LOGE("Unknown vidioc event source");
assert(false);
}
}

@ -7,7 +7,7 @@
#include <media/cam_req_mgr.h>
#include "selfdrive/camerad/cameras/camera_common.h"
#include "selfdrive/common/util.h"
#include "common/util.h"
#define FRAME_BUF_COUNT 4

@ -1,125 +0,0 @@
#include "selfdrive/camerad/cameras/camera_replay.h"
#include <cassert>
#include <thread>
#include "selfdrive/common/clutil.h"
#include "selfdrive/common/util.h"
extern ExitHandler do_exit;
void camera_autoexposure(CameraState *s, float grey_frac) {}
namespace {
const char *BASE_URL = "https://commadataci.blob.core.windows.net/openpilotci/";
const std::string road_camera_route = "0c94aa1e1296d7c6|2021-05-05--19-48-37";
// const std::string driver_camera_route = "534ccd8a0950a00c|2021-06-08--12-15-37";
std::string get_url(std::string route_name, const std::string &camera, int segment_num) {
std::replace(route_name.begin(), route_name.end(), '|', '/');
return util::string_format("%s%s/%d/%s.hevc", BASE_URL, route_name.c_str(), segment_num, camera.c_str());
}
void camera_init(VisionIpcServer *v, CameraState *s, int camera_id, unsigned int fps, cl_device_id device_id, cl_context ctx, VisionStreamType rgb_type, VisionStreamType yuv_type, const std::string &url) {
s->frame = new FrameReader();
if (!s->frame->load(url)) {
printf("failed to load stream from %s", url.c_str());
assert(0);
}
CameraInfo ci = {
.frame_width = (uint32_t)s->frame->width,
.frame_height = (uint32_t)s->frame->height,
.frame_stride = (uint32_t)s->frame->width * 3,
};
s->ci = ci;
s->camera_num = camera_id;
s->fps = fps;
s->buf.init(device_id, ctx, s, v, FRAME_BUF_COUNT, rgb_type, yuv_type);
}
void camera_close(CameraState *s) {
delete s->frame;
}
void run_camera(CameraState *s) {
uint32_t stream_frame_id = 0, frame_id = 0;
size_t buf_idx = 0;
std::unique_ptr<uint8_t[]> rgb_buf = std::make_unique<uint8_t[]>(s->frame->getRGBSize());
std::unique_ptr<uint8_t[]> yuv_buf = std::make_unique<uint8_t[]>(s->frame->getYUVSize());
while (!do_exit) {
if (stream_frame_id == s->frame->getFrameCount()) {
// loop stream
stream_frame_id = 0;
}
if (s->frame->get(stream_frame_id++, rgb_buf.get(), yuv_buf.get())) {
s->buf.camera_bufs_metadata[buf_idx] = {.frame_id = frame_id};
auto &buf = s->buf.camera_bufs[buf_idx];
CL_CHECK(clEnqueueWriteBuffer(buf.copy_q, buf.buf_cl, CL_TRUE, 0, s->frame->getRGBSize(), rgb_buf.get(), 0, NULL, NULL));
s->buf.queue(buf_idx);
++frame_id;
buf_idx = (buf_idx + 1) % FRAME_BUF_COUNT;
}
util::sleep_for(1000 / s->fps);
}
}
void road_camera_thread(CameraState *s) {
util::set_thread_name("replay_road_camera_thread");
run_camera(s);
}
// void driver_camera_thread(CameraState *s) {
// util::set_thread_name("replay_driver_camera_thread");
// run_camera(s);
// }
void process_road_camera(MultiCameraState *s, CameraState *c, int cnt) {
const CameraBuf *b = &c->buf;
MessageBuilder msg;
auto framed = msg.initEvent().initRoadCameraState();
fill_frame_data(framed, b->cur_frame_data);
framed.setImage(kj::arrayPtr((const uint8_t *)b->cur_yuv_buf->addr, b->cur_yuv_buf->len));
framed.setTransform(b->yuv_transform.v);
s->pm->send("roadCameraState", msg);
}
// void process_driver_camera(MultiCameraState *s, CameraState *c, int cnt) {
// MessageBuilder msg;
// auto framed = msg.initEvent().initDriverCameraState();
// framed.setFrameType(cereal::FrameData::FrameType::FRONT);
// fill_frame_data(framed, c->buf.cur_frame_data);
// s->pm->send("driverCameraState", msg);
// }
} // namespace
void cameras_init(VisionIpcServer *v, MultiCameraState *s, cl_device_id device_id, cl_context ctx) {
camera_init(v, &s->road_cam, CAMERA_ID_LGC920, 20, device_id, ctx,
VISION_STREAM_RGB_ROAD, VISION_STREAM_ROAD, get_url(road_camera_route, "fcamera", 0));
// camera_init(v, &s->driver_cam, CAMERA_ID_LGC615, 10, device_id, ctx,
// VISION_STREAM_RGB_DRIVER, VISION_STREAM_DRIVER, get_url(driver_camera_route, "dcamera", 0));
s->pm = new PubMaster({"roadCameraState", "driverCameraState", "thumbnail"});
}
void cameras_open(MultiCameraState *s) {}
void cameras_close(MultiCameraState *s) {
camera_close(&s->road_cam);
camera_close(&s->driver_cam);
delete s->pm;
}
void cameras_run(MultiCameraState *s) {
std::vector<std::thread> threads;
threads.push_back(start_process_thread(s, &s->road_cam, process_road_camera));
// threads.push_back(start_process_thread(s, &s->driver_cam, process_driver_camera));
// threads.push_back(std::thread(driver_camera_thread, &s->driver_cam));
road_camera_thread(&s->road_cam);
for (auto &t : threads) t.join();
cameras_close(s);
}

@ -1,25 +0,0 @@
#pragma once
#include "selfdrive/camerad/cameras/camera_common.h"
#include "selfdrive/ui/replay/framereader.h"
#define FRAME_BUF_COUNT 16
typedef struct CameraState {
int camera_num;
CameraInfo ci;
int fps;
float digital_gain = 0;
CameraBuf buf;
FrameReader *frame = nullptr;
} CameraState;
typedef struct MultiCameraState {
CameraState road_cam;
CameraState driver_cam;
SubMaster *sm = nullptr;
PubMaster *pm = nullptr;
} MultiCameraState;

@ -1,197 +0,0 @@
#include "selfdrive/camerad/cameras/camera_webcam.h"
#include <unistd.h>
#include <cassert>
#include <cstring>
#pragma clang diagnostic push
#pragma clang diagnostic ignored "-Wundefined-inline"
#include <opencv2/core.hpp>
#include <opencv2/highgui.hpp>
#include <opencv2/opencv.hpp>
#include <opencv2/videoio.hpp>
#pragma clang diagnostic pop
#include "selfdrive/common/clutil.h"
#include "selfdrive/common/swaglog.h"
#include "selfdrive/common/timing.h"
#include "selfdrive/common/util.h"
// id of the video capturing device
const int ROAD_CAMERA_ID = util::getenv("ROADCAM_ID", 1);
const int DRIVER_CAMERA_ID = util::getenv("DRIVERCAM_ID", 2);
#define FRAME_WIDTH 1164
#define FRAME_HEIGHT 874
#define FRAME_WIDTH_FRONT 1152
#define FRAME_HEIGHT_FRONT 864
extern ExitHandler do_exit;
namespace {
CameraInfo cameras_supported[CAMERA_ID_MAX] = {
// road facing
[CAMERA_ID_LGC920] = {
.frame_width = FRAME_WIDTH,
.frame_height = FRAME_HEIGHT,
.frame_stride = FRAME_WIDTH*3,
.bayer = false,
.bayer_flip = false,
},
// driver facing
[CAMERA_ID_LGC615] = {
.frame_width = FRAME_WIDTH_FRONT,
.frame_height = FRAME_HEIGHT_FRONT,
.frame_stride = FRAME_WIDTH_FRONT*3,
.bayer = false,
.bayer_flip = false,
},
};
void camera_open(CameraState *s, bool rear) {
// empty
}
void camera_close(CameraState *s) {
// empty
}
void camera_init(VisionIpcServer * v, CameraState *s, int camera_id, unsigned int fps, cl_device_id device_id, cl_context ctx, VisionStreamType rgb_type, VisionStreamType yuv_type) {
assert(camera_id < std::size(cameras_supported));
s->ci = cameras_supported[camera_id];
assert(s->ci.frame_width != 0);
s->camera_num = camera_id;
s->fps = fps;
s->buf.init(device_id, ctx, s, v, FRAME_BUF_COUNT, rgb_type, yuv_type);
}
void run_camera(CameraState *s, cv::VideoCapture &video_cap, float *ts) {
assert(video_cap.isOpened());
cv::Size size(s->ci.frame_width, s->ci.frame_height);
const cv::Mat transform = cv::Mat(3, 3, CV_32F, ts);
uint32_t frame_id = 0;
size_t buf_idx = 0;
while (!do_exit) {
cv::Mat frame_mat, transformed_mat;
video_cap >> frame_mat;
if (frame_mat.empty()) continue;
cv::warpPerspective(frame_mat, transformed_mat, transform, size, cv::INTER_LINEAR, cv::BORDER_CONSTANT, 0);
s->buf.camera_bufs_metadata[buf_idx] = {.frame_id = frame_id};
auto &buf = s->buf.camera_bufs[buf_idx];
int transformed_size = transformed_mat.total() * transformed_mat.elemSize();
CL_CHECK(clEnqueueWriteBuffer(buf.copy_q, buf.buf_cl, CL_TRUE, 0, transformed_size, transformed_mat.data, 0, NULL, NULL));
s->buf.queue(buf_idx);
++frame_id;
buf_idx = (buf_idx + 1) % FRAME_BUF_COUNT;
}
}
static void road_camera_thread(CameraState *s) {
util::set_thread_name("webcam_road_camera_thread");
cv::VideoCapture cap_road(ROAD_CAMERA_ID, cv::CAP_V4L2); // road
cap_road.set(cv::CAP_PROP_FRAME_WIDTH, 853);
cap_road.set(cv::CAP_PROP_FRAME_HEIGHT, 480);
cap_road.set(cv::CAP_PROP_FPS, s->fps);
cap_road.set(cv::CAP_PROP_AUTOFOCUS, 0); // off
cap_road.set(cv::CAP_PROP_FOCUS, 0); // 0 - 255?
// cv::Rect roi_rear(160, 0, 960, 720);
// transforms calculation see tools/webcam/warp_vis.py
float ts[9] = {1.50330396, 0.0, -59.40969163,
0.0, 1.50330396, 76.20704846,
0.0, 0.0, 1.0};
// if camera upside down:
// float ts[9] = {-1.50330396, 0.0, 1223.4,
// 0.0, -1.50330396, 797.8,
// 0.0, 0.0, 1.0};
run_camera(s, cap_road, ts);
}
void driver_camera_thread(CameraState *s) {
cv::VideoCapture cap_driver(DRIVER_CAMERA_ID, cv::CAP_V4L2); // driver
cap_driver.set(cv::CAP_PROP_FRAME_WIDTH, 853);
cap_driver.set(cv::CAP_PROP_FRAME_HEIGHT, 480);
cap_driver.set(cv::CAP_PROP_FPS, s->fps);
// cv::Rect roi_front(320, 0, 960, 720);
// transforms calculation see tools/webcam/warp_vis.py
float ts[9] = {1.42070485, 0.0, -30.16740088,
0.0, 1.42070485, 91.030837,
0.0, 0.0, 1.0};
// if camera upside down:
// float ts[9] = {-1.42070485, 0.0, 1182.2,
// 0.0, -1.42070485, 773.0,
// 0.0, 0.0, 1.0};
run_camera(s, cap_driver, ts);
}
} // namespace
void cameras_init(VisionIpcServer *v, MultiCameraState *s, cl_device_id device_id, cl_context ctx) {
camera_init(v, &s->road_cam, CAMERA_ID_LGC920, 20, device_id, ctx,
VISION_STREAM_RGB_ROAD, VISION_STREAM_ROAD);
camera_init(v, &s->driver_cam, CAMERA_ID_LGC615, 10, device_id, ctx,
VISION_STREAM_RGB_DRIVER, VISION_STREAM_DRIVER);
s->pm = new PubMaster({"roadCameraState", "driverCameraState", "thumbnail"});
}
void camera_autoexposure(CameraState *s, float grey_frac) {}
void cameras_open(MultiCameraState *s) {
// LOG("*** open driver camera ***");
camera_open(&s->driver_cam, false);
// LOG("*** open road camera ***");
camera_open(&s->road_cam, true);
}
void cameras_close(MultiCameraState *s) {
camera_close(&s->road_cam);
camera_close(&s->driver_cam);
delete s->pm;
}
void process_driver_camera(MultiCameraState *s, CameraState *c, int cnt) {
MessageBuilder msg;
auto framed = msg.initEvent().initDriverCameraState();
framed.setFrameType(cereal::FrameData::FrameType::FRONT);
fill_frame_data(framed, c->buf.cur_frame_data);
s->pm->send("driverCameraState", msg);
}
void process_road_camera(MultiCameraState *s, CameraState *c, int cnt) {
const CameraBuf *b = &c->buf;
MessageBuilder msg;
auto framed = msg.initEvent().initRoadCameraState();
fill_frame_data(framed, b->cur_frame_data);
framed.setImage(kj::arrayPtr((const uint8_t *)b->cur_yuv_buf->addr, b->cur_yuv_buf->len));
framed.setTransform(b->yuv_transform.v);
s->pm->send("roadCameraState", msg);
}
void cameras_run(MultiCameraState *s) {
std::vector<std::thread> threads;
threads.push_back(start_process_thread(s, &s->road_cam, process_road_camera));
threads.push_back(start_process_thread(s, &s->driver_cam, process_driver_camera));
std::thread t_rear = std::thread(road_camera_thread, &s->road_cam);
util::set_thread_name("webcam_thread");
driver_camera_thread(&s->driver_cam);
t_rear.join();
for (auto &t : threads) t.join();
cameras_close(s);
}

@ -1,28 +0,0 @@
#pragma once
#ifdef __APPLE__
#include <OpenCL/cl.h>
#else
#include <CL/cl.h>
#endif
#include "selfdrive/camerad/cameras/camera_common.h"
#define FRAME_BUF_COUNT 16
typedef struct CameraState {
CameraInfo ci;
int camera_num;
int fps;
float digital_gain;
CameraBuf buf;
} CameraState;
typedef struct MultiCameraState {
CameraState road_cam;
CameraState driver_cam;
SubMaster *sm;
PubMaster *pm;
} MultiCameraState;

@ -1,189 +1,157 @@
#ifdef HALF_AS_FLOAT
#define half float
#define half3 float3
#else
#pragma OPENCL EXTENSION cl_khr_fp16 : enable
#endif
// post wb CCM
const __constant half3 color_correction_0 = (half3)(1.82717181, -0.31231438, 0.07307673);
const __constant half3 color_correction_1 = (half3)(-0.5743977, 1.36858544, -0.53183455);
const __constant half3 color_correction_2 = (half3)(-0.25277411, -0.05627105, 1.45875782);
// tone mapping params
const half gamma_k = 0.75;
const half gamma_b = 0.125;
const half mp = 0.01; // ideally midpoint should be adaptive
const half rk = 9 - 100*mp;
inline half3 gamma_apply(half3 x) {
#define UV_WIDTH RGB_WIDTH / 2
#define UV_HEIGHT RGB_HEIGHT / 2
#define U_OFFSET RGB_WIDTH * RGB_HEIGHT
#define V_OFFSET RGB_WIDTH * RGB_HEIGHT + UV_WIDTH * UV_HEIGHT
#define RGB_TO_Y(r, g, b) ((((mul24(b, 13) + mul24(g, 65) + mul24(r, 33)) + 64) >> 7) + 16)
#define RGB_TO_U(r, g, b) ((mul24(b, 56) - mul24(g, 37) - mul24(r, 19) + 0x8080) >> 8)
#define RGB_TO_V(r, g, b) ((mul24(r, 56) - mul24(g, 47) - mul24(b, 9) + 0x8080) >> 8)
#define AVERAGE(x, y, z, w) ((convert_ushort(x) + convert_ushort(y) + convert_ushort(z) + convert_ushort(w) + 1) >> 1)
float3 color_correct(float3 rgb) {
// color correction
float3 x = rgb.x * (float3)(1.82717181, -0.31231438, 0.07307673);
x += rgb.y * (float3)(-0.5743977, 1.36858544, -0.53183455);
x += rgb.z * (float3)(-0.25277411, -0.05627105, 1.45875782);
// tone mapping params
const float gamma_k = 0.75;
const float gamma_b = 0.125;
const float mp = 0.01; // ideally midpoint should be adaptive
const float rk = 9 - 100*mp;
// poly approximation for s curve
return (x > mp) ?
((rk * (x-mp) * (1-(gamma_k*mp+gamma_b)) * (1+1/(rk*(1-mp))) / (1+rk*(x-mp))) + gamma_k*mp + gamma_b) :
((rk * (x-mp) * (gamma_k*mp+gamma_b) * (1+1/(rk*mp)) / (1-rk*(x-mp))) + gamma_k*mp + gamma_b);
}
inline half3 color_correct(half3 rgb) {
half3 ret = (half)rgb.x * color_correction_0;
ret += (half)rgb.y * color_correction_1;
ret += (half)rgb.z * color_correction_2;
return gamma_apply(ret);
}
inline half get_vignetting_s(float r) {
float get_vignetting_s(float r) {
if (r < 62500) {
return (half)(1.0f + 0.0000008f*r);
return (1.0f + 0.0000008f*r);
} else if (r < 490000) {
return (half)(0.9625f + 0.0000014f*r);
return (0.9625f + 0.0000014f*r);
} else if (r < 1102500) {
return (half)(1.26434f + 0.0000000000016f*r*r);
return (1.26434f + 0.0000000000016f*r*r);
} else {
return (half)(0.53503625f + 0.0000000000022f*r*r);
return (0.53503625f + 0.0000000000022f*r*r);
}
}
inline half val_from_10(const uchar * source, int gx, int gy, half black_level) {
// parse 12bit
int start = gy * FRAME_STRIDE + (3 * (gx / 2)) + (FRAME_STRIDE * FRAME_OFFSET);
int offset = gx % 2;
uint major = (uint)source[start + offset] << 4;
uint minor = (source[start + 2] >> (4 * offset)) & 0xf;
half pv = (half)((major + minor)/4);
// normalize
pv = max((half)0.0, pv - black_level);
pv /= (1024.0 - black_level);
// correct vignetting
if (CAM_NUM == 1) { // fcamera
gx = (gx - RGB_WIDTH/2);
gy = (gy - RGB_HEIGHT/2);
pv *= get_vignetting_s(gx*gx + gy*gy);
}
pv = clamp(pv, (half)0.0, (half)1.0);
return pv;
float4 val4_from_12(uchar8 pvs, float gain) {
uint4 parsed = (uint4)(((uint)pvs.s0<<4) + (pvs.s1>>4), // is from the previous 10 bit
((uint)pvs.s2<<4) + (pvs.s4&0xF),
((uint)pvs.s3<<4) + (pvs.s4>>4),
((uint)pvs.s5<<4) + (pvs.s7&0xF));
// normalize and scale
float4 pv = (convert_float4(parsed) - 168.0) / (4096.0 - 168.0);
return clamp(pv*gain, 0.0, 1.0);
}
inline half get_k(half a, half b, half c, half d) {
float get_k(float a, float b, float c, float d) {
return 2.0 - (fabs(a - b) + fabs(c - d));
}
__kernel void debayer10(const __global uchar * in,
__global uchar * out,
__local half * cached,
float black_level
)
__kernel void debayer10(const __global uchar * in, __global uchar * out)
{
const int x_global = get_global_id(0);
const int y_global = get_global_id(1);
const int x_local = get_local_id(0);
const int y_local = get_local_id(1);
const int localRowLen = 2 + get_local_size(0); // 2 padding
const int localColLen = 2 + get_local_size(1);
const int gid_x = get_global_id(0);
const int gid_y = get_global_id(1);
const int localOffset = (y_local + 1) * localRowLen + x_local + 1;
const int y_top_mod = (gid_y == 0) ? 2: 0;
const int y_bot_mod = (gid_y == (RGB_HEIGHT/2 - 1)) ? 1: 3;
int out_idx = 3 * x_global + 3 * y_global * RGB_WIDTH;
float3 rgb;
uchar3 rgb_out[4];
// cache padding
int localColOffset = -1;
int globalColOffset;
int start = (2 * gid_y - 1) * FRAME_STRIDE + (3 * gid_x - 2) + (FRAME_STRIDE * FRAME_OFFSET);
const int x_global_mod = (x_global == 0 || x_global == RGB_WIDTH - 1) ? -1: 1;
const int y_global_mod = (y_global == 0 || y_global == RGB_HEIGHT - 1) ? -1: 1;
// read in 8x4 chars
uchar8 dat[4];
dat[0] = vload8(0, in + start + FRAME_STRIDE*y_top_mod);
dat[1] = vload8(0, in + start + FRAME_STRIDE*1);
dat[2] = vload8(0, in + start + FRAME_STRIDE*2);
dat[3] = vload8(0, in + start + FRAME_STRIDE*y_bot_mod);
half pv = val_from_10(in, x_global, y_global, black_level);
cached[localOffset] = pv;
// cache padding
if (x_local < 1) {
localColOffset = x_local;
globalColOffset = -1;
cached[(y_local + 1) * localRowLen + x_local] = val_from_10(in, x_global-x_global_mod, y_global, black_level);
} else if (x_local >= get_local_size(0) - 1) {
localColOffset = x_local + 2;
globalColOffset = 1;
cached[localOffset + 1] = val_from_10(in, x_global+x_global_mod, y_global, black_level);
}
if (y_local < 1) {
cached[y_local * localRowLen + x_local + 1] = val_from_10(in, x_global, y_global-y_global_mod, black_level);
if (localColOffset != -1) {
cached[y_local * localRowLen + localColOffset] = val_from_10(in, x_global+(x_global_mod*globalColOffset), y_global-y_global_mod, black_level);
}
} else if (y_local >= get_local_size(1) - 1) {
cached[(y_local + 2) * localRowLen + x_local + 1] = val_from_10(in, x_global, y_global+y_global_mod, black_level);
if (localColOffset != -1) {
cached[(y_local + 2) * localRowLen + localColOffset] = val_from_10(in, x_global+(x_global_mod*globalColOffset), y_global+y_global_mod, black_level);
}
// correct vignetting
#if VIGNETTING
int gx = (gid_x*2 - RGB_WIDTH/2);
int gy = (gid_y*2 - RGB_HEIGHT/2);
const float gain = get_vignetting_s(gx*gx + gy*gy);
#else
const float gain = 1.0;
#endif
// process them to floats
float4 va = val4_from_12(dat[0], gain);
float4 vb = val4_from_12(dat[1], gain);
float4 vc = val4_from_12(dat[2], gain);
float4 vd = val4_from_12(dat[3], gain);
if (gid_x == 0) {
va.s0 = va.s2;
vb.s0 = vb.s2;
vc.s0 = vc.s2;
vd.s0 = vd.s2;
} else if (gid_x == RGB_WIDTH/2 - 1) {
va.s3 = va.s1;
vb.s3 = vb.s1;
vc.s3 = vc.s1;
vd.s3 = vd.s1;
}
// sync
barrier(CLK_LOCAL_MEM_FENCE);
half d1 = cached[localOffset - localRowLen - 1];
half d2 = cached[localOffset - localRowLen + 1];
half d3 = cached[localOffset + localRowLen - 1];
half d4 = cached[localOffset + localRowLen + 1];
half n1 = cached[localOffset - localRowLen];
half n2 = cached[localOffset + 1];
half n3 = cached[localOffset + localRowLen];
half n4 = cached[localOffset - 1];
half3 rgb;
// a simplified version of https://opensignalprocessingjournal.com/contents/volumes/V6/TOSIGPJ-6-1/TOSIGPJ-6-1.pdf
if (x_global % 2 == 0) {
if (y_global % 2 == 0) {
rgb.y = pv; // G1(R)
half k1 = get_k(d1, pv, d2, pv);
half k2 = get_k(d2, pv, d4, pv);
half k3 = get_k(d3, pv, d4, pv);
half k4 = get_k(d1, pv, d3, pv);
// R_G1
rgb.x = (k2*n2+k4*n4)/(k2+k4);
// B_G1
rgb.z = (k1*n1+k3*n3)/(k1+k3);
} else {
rgb.z = pv; // B
half k1 = get_k(d1, d3, d2, d4);
half k2 = get_k(n1, n4, n2, n3);
half k3 = get_k(d1, d2, d3, d4);
half k4 = get_k(n1, n2, n3, n4);
// G_B
rgb.y = (k1*(n1+n3)*0.5+k3*(n2+n4)*0.5)/(k1+k3);
// R_B
rgb.x = (k2*(d2+d3)*0.5+k4*(d1+d4)*0.5)/(k2+k4);
}
} else {
if (y_global % 2 == 0) {
rgb.x = pv; // R
half k1 = get_k(d1, d3, d2, d4);
half k2 = get_k(n1, n4, n2, n3);
half k3 = get_k(d1, d2, d3, d4);
half k4 = get_k(n1, n2, n3, n4);
// G_R
rgb.y = (k1*(n1+n3)*0.5+k3*(n2+n4)*0.5)/(k1+k3);
// B_R
rgb.z = (k2*(d2+d3)*0.5+k4*(d1+d4)*0.5)/(k2+k4);
} else {
rgb.y = pv; // G2(B)
half k1 = get_k(d1, pv, d2, pv);
half k2 = get_k(d2, pv, d4, pv);
half k3 = get_k(d3, pv, d4, pv);
half k4 = get_k(d1, pv, d3, pv);
// R_G2
rgb.x = (k1*n1+k3*n3)/(k1+k3);
// B_G2
rgb.z = (k2*n2+k4*n4)/(k2+k4);
}
}
uchar3 rgbc = convert_uchar3_sat(color_correct(clamp(rgb, (half)0.0, (half)1.0)) * 255.0);
out[out_idx + 0] = rgbc.z;
out[out_idx + 1] = rgbc.y;
out[out_idx + 2] = rgbc.x;
const float k01 = get_k(va.s0, vb.s1, va.s2, vb.s1);
const float k02 = get_k(va.s2, vb.s1, vc.s2, vb.s1);
const float k03 = get_k(vc.s0, vb.s1, vc.s2, vb.s1);
const float k04 = get_k(va.s0, vb.s1, vc.s0, vb.s1);
rgb.x = (k02*vb.s2+k04*vb.s0)/(k02+k04); // R_G1
rgb.y = vb.s1; // G1(R)
rgb.z = (k01*va.s1+k03*vc.s1)/(k01+k03); // B_G1
rgb_out[0] = convert_uchar3_sat(color_correct(clamp(rgb, 0.0, 1.0)) * 255.0);
const float k11 = get_k(va.s1, vc.s1, va.s3, vc.s3);
const float k12 = get_k(va.s2, vb.s1, vb.s3, vc.s2);
const float k13 = get_k(va.s1, va.s3, vc.s1, vc.s3);
const float k14 = get_k(va.s2, vb.s3, vc.s2, vb.s1);
rgb.x = vb.s2; // R
rgb.y = (k11*(va.s2+vc.s2)*0.5+k13*(vb.s3+vb.s1)*0.5)/(k11+k13); // G_R
rgb.z = (k12*(va.s3+vc.s1)*0.5+k14*(va.s1+vc.s3)*0.5)/(k12+k14); // B_R
rgb_out[1] = convert_uchar3_sat(color_correct(clamp(rgb, 0.0, 1.0)) * 255.0);
const float k21 = get_k(vb.s0, vd.s0, vb.s2, vd.s2);
const float k22 = get_k(vb.s1, vc.s0, vc.s2, vd.s1);
const float k23 = get_k(vb.s0, vb.s2, vd.s0, vd.s2);
const float k24 = get_k(vb.s1, vc.s2, vd.s1, vc.s0);
rgb.x = (k22*(vb.s2+vd.s0)*0.5+k24*(vb.s0+vd.s2)*0.5)/(k22+k24); // R_B
rgb.y = (k21*(vb.s1+vd.s1)*0.5+k23*(vc.s2+vc.s0)*0.5)/(k21+k23); // G_B
rgb.z = vc.s1; // B
rgb_out[2] = convert_uchar3_sat(color_correct(clamp(rgb, 0.0, 1.0)) * 255.0);
const float k31 = get_k(vb.s1, vc.s2, vb.s3, vc.s2);
const float k32 = get_k(vb.s3, vc.s2, vd.s3, vc.s2);
const float k33 = get_k(vd.s1, vc.s2, vd.s3, vc.s2);
const float k34 = get_k(vb.s1, vc.s2, vd.s1, vc.s2);
rgb.x = (k31*vb.s2+k33*vd.s2)/(k31+k33); // R_G2
rgb.y = vc.s2; // G2(B)
rgb.z = (k32*vc.s3+k34*vc.s1)/(k32+k34); // B_G2
rgb_out[3] = convert_uchar3_sat(color_correct(clamp(rgb, 0.0, 1.0)) * 255.0);
// write ys
uchar2 yy = (uchar2)(
RGB_TO_Y(rgb_out[0].s0, rgb_out[0].s1, rgb_out[0].s2),
RGB_TO_Y(rgb_out[1].s0, rgb_out[1].s1, rgb_out[1].s2)
);
vstore2(yy, 0, out + mad24(gid_y * 2, RGB_WIDTH, gid_x * 2));
yy = (uchar2)(
RGB_TO_Y(rgb_out[2].s0, rgb_out[2].s1, rgb_out[2].s2),
RGB_TO_Y(rgb_out[3].s0, rgb_out[3].s1, rgb_out[3].s2)
);
vstore2(yy, 0, out + mad24(gid_y * 2 + 1, RGB_WIDTH, gid_x * 2));
// write uvs
const short ar = AVERAGE(rgb_out[0].s0, rgb_out[1].s0, rgb_out[2].s0, rgb_out[3].s0);
const short ag = AVERAGE(rgb_out[0].s1, rgb_out[1].s1, rgb_out[2].s1, rgb_out[3].s1);
const short ab = AVERAGE(rgb_out[0].s2, rgb_out[1].s2, rgb_out[2].s2, rgb_out[3].s2);
out[U_OFFSET + mad24(gid_y, UV_WIDTH, gid_x)] = RGB_TO_U(ar, ag, ab);
out[V_OFFSET + mad24(gid_y, UV_WIDTH, gid_x)] = RGB_TO_V(ar, ag, ab);
}

File diff suppressed because one or more lines are too long

@ -4,7 +4,7 @@
#include <cstdint>
#include <vector>
#include "selfdrive/common/clutil.h"
#include "common/clutil.h"
#define NUM_SEGMENTS_X 8
#define NUM_SEGMENTS_Y 6

@ -2,8 +2,8 @@
#include <cassert>
#include "selfdrive/common/params.h"
#include "selfdrive/common/util.h"
#include "common/params.h"
#include "common/util.h"
#include "selfdrive/hardware/hw.h"
int main(int argc, char *argv[]) {

@ -17,9 +17,9 @@ from selfdrive.manager.process_config import managed_processes
LM_THRESH = 120 # defined in selfdrive/camerad/imgproc/utils.h
VISION_STREAMS = {
"roadCameraState": VisionStreamType.VISION_STREAM_RGB_ROAD,
"driverCameraState": VisionStreamType.VISION_STREAM_RGB_DRIVER,
"wideRoadCameraState": VisionStreamType.VISION_STREAM_RGB_WIDE_ROAD,
"roadCameraState": VisionStreamType.VISION_STREAM_ROAD,
"driverCameraState": VisionStreamType.VISION_STREAM_DRIVER,
"wideRoadCameraState": VisionStreamType.VISION_STREAM_WIDE_ROAD,
}
@ -28,12 +28,28 @@ def jpeg_write(fn, dat):
img.save(fn, "JPEG")
def extract_image(buf, w, h, stride):
img = np.hstack([buf[i * stride:i * stride + 3 * w] for i in range(h)])
b = img[::3].reshape(h, w)
g = img[1::3].reshape(h, w)
r = img[2::3].reshape(h, w)
return np.dstack([r, g, b])
def yuv_to_rgb(y, u, v):
ul = np.repeat(np.repeat(u, 2).reshape(u.shape[0], y.shape[1]), 2, axis=0).reshape(y.shape)
vl = np.repeat(np.repeat(v, 2).reshape(v.shape[0], y.shape[1]), 2, axis=0).reshape(y.shape)
yuv = np.dstack((y, ul, vl)).astype(np.int16)
yuv[:, :, 1:] -= 128
m = np.array([
[1.00000, 1.00000, 1.00000],
[0.00000, -0.39465, 2.03211],
[1.13983, -0.58060, 0.00000],
])
rgb = np.dot(yuv, m)
return rgb.astype(np.uint8)
def extract_image(buf, w, h):
y = np.array(buf[:w*h], dtype=np.uint8).reshape((h, w))
u = np.array(buf[w*h: w*h+(w//2)*(h//2)], dtype=np.uint8).reshape((h//2, w//2))
v = np.array(buf[w*h+(w//2)*(h//2):], dtype=np.uint8).reshape((h//2, w//2))
return yuv_to_rgb(y, u, v)
def rois_in_focus(lapres: List[float]) -> float:
@ -63,10 +79,10 @@ def get_snapshots(frame="roadCameraState", front_frame="driverCameraState", focu
rear, front = None, None
if frame is not None:
c = vipc_clients[frame]
rear = extract_image(c.recv(), c.width, c.height, c.stride)
rear = extract_image(c.recv(), c.width, c.height)
if front_frame is not None:
c = vipc_clients[front_frame]
front = extract_image(c.recv(), c.width, c.height, c.stride)
front = extract_image(c.recv(), c.width, c.height)
return rear, front

@ -7,15 +7,9 @@
#include <cmath>
#include <cstring>
#include "selfdrive/common/util.h"
#include "common/util.h"
#include "selfdrive/camerad/cameras/camera_common.h"
// needed by camera_common.cc
void camera_autoexposure(CameraState *s, float grey_frac) {}
void cameras_init(VisionIpcServer *v, MultiCameraState *s, cl_device_id device_id, cl_context ctx) {}
void cameras_open(MultiCameraState *s) {}
void cameras_run(MultiCameraState *s) {}
int main() {
// set up fake camerabuf
CameraBuf cb = {};

@ -0,0 +1,26 @@
// TODO: cleanup AE tests
// needed by camera_common.cc
void camera_autoexposure(CameraState *s, float grey_frac) {}
void cameras_init(VisionIpcServer *v, MultiCameraState *s, cl_device_id device_id, cl_context ctx) {}
void cameras_open(MultiCameraState *s) {}
void cameras_run(MultiCameraState *s) {}
typedef struct CameraState {
int camera_num;
CameraInfo ci;
int fps;
float digital_gain = 0;
CameraBuf buf;
} CameraState;
typedef struct MultiCameraState {
CameraState road_cam;
CameraState driver_cam;
SubMaster *sm = nullptr;
PubMaster *pm = nullptr;
} MultiCameraState;

@ -1,6 +1,6 @@
#pragma once
#include "selfdrive/common/clutil.h"
#include "common/clutil.h"
class Rgb2Yuv {
public:

@ -31,7 +31,7 @@
#include "libyuv.h"
#include "selfdrive/camerad/transforms/rgb_to_yuv.h"
#include "selfdrive/common/clutil.h"
#include "common/clutil.h"
static inline double millis_since_boot() {
struct timespec t;

@ -9,7 +9,6 @@ class CarController:
self.CP = CP
self.frame = 0
self.last_angle = 0
self.long_control_counter = 0
self.packer = CANPacker(dbc_name)
self.pt_packer = CANPacker(DBC[CP.carFingerprint]['pt'])
self.tesla_can = TeslaCAN(self.packer, self.pt_packer)
@ -41,15 +40,15 @@ class CarController:
self.last_angle = apply_angle
can_sends.append(self.tesla_can.create_steering_control(apply_angle, lkas_enabled, self.frame))
# Longitudinal control (40Hz)
if self.CP.openpilotLongitudinalControl and self.frame % 5 in (0, 2):
# Longitudinal control (in sync with stock message, about 40Hz)
if self.CP.openpilotLongitudinalControl:
target_accel = actuators.accel
target_speed = max(CS.out.vEgo + (target_accel * CarControllerParams.ACCEL_TO_SPEED_MULTIPLIER), 0)
max_accel = 0 if target_accel < 0 else target_accel
min_accel = 0 if target_accel > 0 else target_accel
can_sends.extend(self.tesla_can.create_longitudinal_commands(CS.acc_state, target_speed, min_accel, max_accel, self.long_control_counter))
self.long_control_counter += 1
while len(CS.das_control_counters) > 0:
can_sends.extend(self.tesla_can.create_longitudinal_commands(CS.acc_state, target_speed, min_accel, max_accel, CS.das_control_counters.popleft()))
# Cancel on user steering override, since there is no steering torque blending
if hands_on_fault:

@ -1,4 +1,5 @@
import copy
from collections import deque
from cereal import car
from common.conversions import Conversions as CV
from selfdrive.car.tesla.values import DBC, CANBUS, GEAR_MAP, DOORS, BUTTONS
@ -17,6 +18,7 @@ class CarState(CarStateBase):
self.hands_on_level = 0
self.steer_warning = None
self.acc_state = 0
self.das_control_counters = deque(maxlen=32)
def update(self, cp, cp_cam):
ret = car.CarState.new_message()
@ -87,9 +89,13 @@ class CarState(CarStateBase):
# TODO: blindspot
# AEB
ret.stockAeb = (cp_cam.vl["DAS_control"]["DAS_aebEvent"] == 1)
# Messages needed by carcontroller
self.msg_stw_actn_req = copy.copy(cp.vl["STW_ACTN_RQ"])
self.acc_state = cp_cam.vl["DAS_control"]["DAS_accState"]
self.das_control_counters.extend(cp_cam.vl_all["DAS_control"]["DAS_controlCounter"])
return ret
@ -177,6 +183,8 @@ class CarState(CarStateBase):
signals = [
# sig_name, sig_address
("DAS_accState", "DAS_control"),
("DAS_aebEvent", "DAS_control"),
("DAS_controlCounter", "DAS_control"),
]
checks = [
# sig_address, frequency

@ -51,7 +51,7 @@ class TeslaCAN:
"DAS_jerkMax": CarControllerParams.JERK_LIMIT_MAX,
"DAS_accelMin": min_accel,
"DAS_accelMax": max_accel,
"DAS_controlCounter": (cnt % 8),
"DAS_controlCounter": cnt,
"DAS_controlChecksum": 0,
}

@ -29,7 +29,7 @@ non_tested_cars = [
TestRoute = namedtuple('TestRoute', ['route', 'car_fingerprint', 'segment'], defaults=(None,))
routes = [
TestRoute("d6ac8ebdb47bc549|2022-03-31--13-10-06", COMMA.BODY),
TestRoute("efdf9af95e71cd84|2022-05-13--19-03-31", COMMA.BODY),
TestRoute("0c94aa1e1296d7c6|2021-05-05--19-48-37", CHRYSLER.JEEP_CHEROKEE),
TestRoute("91dfedae61d7bd75|2021-05-22--20-07-52", CHRYSLER.JEEP_CHEROKEE_2019),
@ -127,6 +127,7 @@ routes = [
TestRoute("b14c5b4742e6fc85|2020-07-28--19-50-11", TOYOTA.RAV4),
TestRoute("32a7df20486b0f70|2020-02-06--16-06-50", TOYOTA.RAV4H),
TestRoute("cdf2f7de565d40ae|2019-04-25--03-53-41", TOYOTA.RAV4_TSS2),
TestRoute("a5c341bb250ca2f0|2022-05-18--16-05-17", TOYOTA.RAV4_TSS2_2022),
TestRoute("7e34a988419b5307|2019-12-18--19-13-30", TOYOTA.RAV4H_TSS2),
TestRoute("2475fb3eb2ffcc2e|2022-04-29--12-46-23", TOYOTA.RAV4H_TSS2_2022),
TestRoute("e6a24be49a6cd46e|2019-10-29--10-52-42", TOYOTA.LEXUS_ES_TSS2),

@ -38,6 +38,8 @@ class TestCarInterfaces(unittest.TestCase):
tuning = car_params.lateralTuning.which()
if tuning == 'pid':
self.assertTrue(len(car_params.lateralTuning.pid.kpV))
elif tuning == 'lqr':
self.assertTrue(len(car_params.lateralTuning.lqr.a))
elif tuning == 'torque':
self.assertTrue(car_params.lateralTuning.torque.kf > 0)
elif tuning == 'indi':

@ -19,13 +19,16 @@ class TestCarDocs(unittest.TestCase):
def test_naming_conventions(self):
# Asserts market-standard car naming conventions by make
for car in self.all_cars:
tokens = car.model.lower().split(" ")
if car.car_name == "hyundai":
tokens = car.model.lower().split(" ")
self.assertNotIn("phev", tokens, "Use `Plug-in Hybrid`")
self.assertNotIn("hev", tokens, "Use `Hybrid`")
self.assertNotIn("ev", tokens, "Use `Electric`")
if "plug-in hybrid" in car.model.lower():
self.assertIn("Plug-in Hybrid", car.model, "Use correct capitalization")
elif car.car_name == "toyota":
if "rav4" in tokens:
self.assertIn("RAV4", car.model, "Use correct capitalization")
if __name__ == "__main__":

@ -116,6 +116,8 @@ class TestCarModel(unittest.TestCase):
self.assertTrue(len(self.CP.lateralTuning.pid.kpV))
elif tuning == 'torque':
self.assertTrue(self.CP.lateralTuning.torque.kf > 0)
elif tuning == 'lqr':
self.assertTrue(len(self.CP.lateralTuning.lqr.a))
elif tuning == 'indi':
self.assertTrue(len(self.CP.lateralTuning.indi.outerLoopGainV))
else:

@ -47,7 +47,7 @@ class CarInterface(CarInterfaceBase):
ret.steerRatio = 17.4
tire_stiffness_factor = 0.5533
ret.mass = 3340. * CV.LB_TO_KG + STD_CARGO_KG
set_lat_tune(ret.lateralTuning, LatTunes.TORQUE, MAX_LAT_ACCEL=1.8, FRICTION=0.06)
set_lat_tune(ret.lateralTuning, LatTunes.LQR_RAV4)
elif candidate in (CAR.RAV4, CAR.RAV4H):
stop_and_go = True if (candidate in CAR.RAV4H) else False
@ -55,7 +55,7 @@ class CarInterface(CarInterfaceBase):
ret.steerRatio = 16.88 # 14.5 is spec end-to-end
tire_stiffness_factor = 0.5533
ret.mass = 3650. * CV.LB_TO_KG + STD_CARGO_KG # mean between normal and hybrid
set_lat_tune(ret.lateralTuning, LatTunes.TORQUE, MAX_LAT_ACCEL=1.8, FRICTION=0.06)
set_lat_tune(ret.lateralTuning, LatTunes.LQR_RAV4)
elif candidate == CAR.COROLLA:
ret.wheelbase = 2.70
@ -87,10 +87,7 @@ class CarInterface(CarInterfaceBase):
ret.steerRatio = 13.7
tire_stiffness_factor = 0.7933
ret.mass = 3400. * CV.LB_TO_KG + STD_CARGO_KG # mean between normal and hybrid
if candidate in (CAR.CAMRY_TSS2, CAR.CAMRYH_TSS2):
set_lat_tune(ret.lateralTuning, LatTunes.TORQUE, MAX_LAT_ACCEL=2.4, FRICTION=0.05)
else:
set_lat_tune(ret.lateralTuning, LatTunes.PID_C)
set_lat_tune(ret.lateralTuning, LatTunes.PID_C)
elif candidate in (CAR.HIGHLANDER_TSS2, CAR.HIGHLANDERH_TSS2):
stop_and_go = True
@ -118,7 +115,7 @@ class CarInterface(CarInterfaceBase):
ret.mass = 3505. * CV.LB_TO_KG + STD_CARGO_KG # mean between normal and hybrid
set_lat_tune(ret.lateralTuning, LatTunes.PID_H)
elif candidate in (CAR.RAV4_TSS2, CAR.RAV4H_TSS2, CAR.RAV4H_TSS2_2022):
elif candidate in (CAR.RAV4_TSS2, CAR.RAV4_TSS2_2022, CAR.RAV4H_TSS2, CAR.RAV4H_TSS2_2022):
stop_and_go = True
ret.wheelbase = 2.68986
ret.steerRatio = 14.3
@ -139,7 +136,7 @@ class CarInterface(CarInterfaceBase):
ret.steerRatio = 13.9
tire_stiffness_factor = 0.444 # not optimized yet
ret.mass = 3060. * CV.LB_TO_KG + STD_CARGO_KG
set_lat_tune(ret.lateralTuning, LatTunes.TORQUE, MAX_LAT_ACCEL=2.0, FRICTION=0.07)
set_lat_tune(ret.lateralTuning, LatTunes.PID_D)
elif candidate in (CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2, CAR.LEXUS_ESH):
stop_and_go = True

@ -58,6 +58,16 @@ def set_lat_tune(tune, name, MAX_LAT_ACCEL=2.5, FRICTION=.1):
tune.torque.kf = 1.0 / MAX_LAT_ACCEL
tune.torque.ki = 0.1 / MAX_LAT_ACCEL
tune.torque.friction = FRICTION
elif name == LatTunes.LQR_RAV4:
tune.init('lqr')
tune.lqr.scale = 1500.0
tune.lqr.ki = 0.05
tune.lqr.a = [0., 1., -0.22619643, 1.21822268]
tune.lqr.b = [-1.92006585e-04, 3.95603032e-05]
tune.lqr.c = [1., 0.]
tune.lqr.k = [-110.73572306, 451.22718255]
tune.lqr.l = [0.3233671, 0.3185757]
tune.lqr.dcGain = 0.002237852961363602
elif name == LatTunes.INDI_PRIUS:
tune.init('indi')
tune.indi.innerLoopGainBP = [0.]

@ -62,6 +62,7 @@ class CAR:
RAV4 = "TOYOTA RAV4 2017"
RAV4H = "TOYOTA RAV4 HYBRID 2017"
RAV4_TSS2 = "TOYOTA RAV4 2019"
RAV4_TSS2_2022 = "TOYOTA RAV4 2022"
RAV4H_TSS2 = "TOYOTA RAV4 HYBRID 2019"
RAV4H_TSS2_2022 = "TOYOTA RAV4 HYBRID 2022"
MIRAI = "TOYOTA MIRAI 2021" # TSS 2.5
@ -143,6 +144,7 @@ CAR_INFO: Dict[str, Union[ToyotaCarInfo, List[ToyotaCarInfo]]] = {
CAR.RAV4: ToyotaCarInfo("Toyota RAV4 2016-18", "TSS-P", footnotes=[Footnote.DSU]),
CAR.RAV4H: ToyotaCarInfo("Toyota RAV4 Hybrid 2016-18", "TSS-P", footnotes=[Footnote.DSU]),
CAR.RAV4_TSS2: ToyotaCarInfo("Toyota RAV4 2019-21", video_link="https://www.youtube.com/watch?v=wJxjDd42gGA"),
CAR.RAV4_TSS2_2022: ToyotaCarInfo("Toyota RAV4 2022"),
CAR.RAV4H_TSS2: ToyotaCarInfo("Toyota RAV4 Hybrid 2019-21"),
CAR.RAV4H_TSS2_2022: ToyotaCarInfo("Toyota RAV4 Hybrid 2022"),
CAR.MIRAI: ToyotaCarInfo("Toyota Mirai 2021"),
@ -1276,6 +1278,25 @@ FW_VERSIONS = {
b'\x028646F4203800\x00\x00\x00\x008646G2601500\x00\x00\x00\x00',
],
},
CAR.RAV4_TSS2_2022: {
(Ecu.esp, 0x7b0, None): [
b'\x01F15260R350\x00\x00\x00\x00\x00\x00',
b'\x01F15260R361\x00\x00\x00\x00\x00\x00',
],
(Ecu.eps, 0x7a1, None): [
b'\x028965B0R01500\x00\x00\x00\x008965B0R02500\x00\x00\x00\x00',
],
(Ecu.engine, 0x700, None): [
b'\x01896634AA1000\x00\x00\x00\x00',
b'\x01896634A88000\x00\x00\x00\x00',
],
(Ecu.fwdRadar, 0x750, 0xf): [
b'\x018821F0R01100\x00\x00\x00\x00',
],
(Ecu.fwdCamera, 0x750, 0x6d): [
b'\x028646F0R02100\x00\x00\x00\x008646G0R01100\x00\x00\x00\x00',
],
},
CAR.RAV4H_TSS2: {
(Ecu.engine, 0x700, None): [
b'\x01896634A15000\x00\x00\x00\x00',
@ -1718,6 +1739,7 @@ FW_VERSIONS = {
b'\x01896634D12000\x00\x00\x00\x00',
b'\x01896634D12100\x00\x00\x00\x00',
b'\x01896634D43000\x00\x00\x00\x00',
b'\x01896630ED0100\x00\x00\x00\x00',
],
(Ecu.esp, 0x7b0, None): [
b'\x01F15260E031\x00\x00\x00\x00\x00\x00',
@ -1865,6 +1887,7 @@ DBC = {
CAR.AVALON_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'),
CAR.AVALONH_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'),
CAR.RAV4_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'),
CAR.RAV4_TSS2_2022: dbc_dict('toyota_nodsu_pt_generated', None),
CAR.COROLLA_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'),
CAR.COROLLAH_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'),
CAR.LEXUS_ES_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'),
@ -1888,14 +1911,14 @@ DBC = {
EPS_SCALE = defaultdict(lambda: 73, {CAR.PRIUS: 66, CAR.COROLLA: 88, CAR.LEXUS_IS: 77, CAR.LEXUS_RC: 77, CAR.LEXUS_CTH: 100, CAR.PRIUS_V: 100})
# Toyota/Lexus Safety Sense 2.0 and 2.5
TSS2_CAR = {CAR.RAV4_TSS2, CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2, CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2, CAR.RAV4H_TSS2, CAR.RAV4H_TSS2_2022,
TSS2_CAR = {CAR.RAV4_TSS2, CAR.RAV4_TSS2_2022, CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2, CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2, CAR.RAV4H_TSS2, CAR.RAV4H_TSS2_2022,
CAR.LEXUS_RX_TSS2, CAR.LEXUS_RXH_TSS2, CAR.HIGHLANDER_TSS2, CAR.HIGHLANDERH_TSS2, CAR.PRIUS_TSS2, CAR.CAMRY_TSS2, CAR.CAMRYH_TSS2,
CAR.MIRAI, CAR.LEXUS_NX_TSS2, CAR.ALPHARD_TSS2, CAR.AVALON_TSS2, CAR.AVALONH_TSS2, CAR.ALPHARDH_TSS2}
NO_DSU_CAR = TSS2_CAR | {CAR.CHR, CAR.CHRH, CAR.CAMRY, CAR.CAMRYH}
# these cars have a radar which sends ACC messages instead of the camera
RADAR_ACC_CAR = {CAR.RAV4H_TSS2_2022}
RADAR_ACC_CAR = {CAR.RAV4H_TSS2_2022, CAR.RAV4_TSS2_2022}
EV_HYBRID_CAR = {CAR.AVALONH_2019, CAR.AVALONH_TSS2, CAR.CAMRYH, CAR.CAMRYH_TSS2, CAR.CHRH, CAR.COROLLAH_TSS2, CAR.HIGHLANDERH, CAR.HIGHLANDERH_TSS2, CAR.PRIUS,
CAR.PRIUS_V, CAR.RAV4H, CAR.RAV4H_TSS2, CAR.RAV4H_TSS2_2022, CAR.LEXUS_CTH, CAR.MIRAI, CAR.LEXUS_ESH, CAR.LEXUS_ESH_TSS2, CAR.LEXUS_NXH, CAR.LEXUS_RXH,

@ -469,16 +469,16 @@ FW_VERSIONS = {
},
CAR.POLO_MK6: {
(Ecu.engine, 0x7e0, None): [
b'\xf1\x8704C906025H \xf1\x895177',
b'\xf1\x8704C906025H \xf1\x895177',
],
(Ecu.transmission, 0x7e1, None): [
b'\xf1\x870CW300050D \xf1\x891908',
b'\xf1\x870CW300050D \xf1\x891908',
],
(Ecu.srs, 0x715, None): [
b'\xf1\x872Q0959655AJ\xf1\x890250\xf1\x82\x1248130411110416--04040404784811152H14',
],
(Ecu.eps, 0x712, None): [
b'\xf1\x872Q1909144M \xf1\x896041',
b'\xf1\x872Q1909144M \xf1\x896041',
],
(Ecu.fwdRadar, 0x757, None): [
b'\xf1\x872Q0907572R \xf1\x890372',
@ -486,19 +486,23 @@ FW_VERSIONS = {
},
CAR.TAOS_MK1: {
(Ecu.engine, 0x7e0, None): [
b'\xf1\x8705E906013E \xf1\x891624',
b'\xf1\x8704E906027NJ\xf1\x891445',
b'\xf1\x8705E906013E \xf1\x891624',
],
(Ecu.transmission, 0x7e1, None): [
b'\xf1\x8709S927158BL\xf1\x893791',
b'\xf1\x8709S927158BL\xf1\x893791',
b'\xf1\x8709S927158FF\xf1\x893876',
],
(Ecu.fwdRadar, 0x757, None): [
b'\xf1\x872Q0907572T \xf1\x890383',
(Ecu.srs, 0x715, None): [
b'\xf1\x875Q0959655CB\xf1\x890421\xf1\x82\x1311111111333500314646021450149333613100',
b'\xf1\x875Q0959655CE\xf1\x890421\xf1\x82\x1311110011333300314240021350139333613100',
],
(Ecu.eps, 0x712, None): [
b'\xf1\x875QM909144C \xf1\x891082\xf1\x82\x0521060605A1',
b'\xf1\x875QM909144C \xf1\x891082\xf1\x82\x0521060405A1',
b'\xf1\x875QM909144C \xf1\x891082\xf1\x82\x0521060605A1',
],
(Ecu.srs, 0x715, None): [
b'\xf1\x875Q0959655CB\xf1\x890421\xf1\x82\x1311111111333500314646021450149333613100',
(Ecu.fwdRadar, 0x757, None): [
b'\xf1\x872Q0907572T \xf1\x890383',
],
},
CAR.TCROSS_MK1: {

@ -16,8 +16,8 @@
#include <chrono>
#include "cereal/messaging/messaging.h"
#include "selfdrive/common/timing.h"
#include "selfdrive/common/util.h"
#include "common/timing.h"
#include "common/util.h"
ExitHandler do_exit;

@ -1,35 +0,0 @@
Import('env', 'arch', 'SHARED')
if SHARED:
fxn = env.SharedLibrary
else:
fxn = env.Library
common_libs = [
'params.cc',
'statlog.cc',
'swaglog.cc',
'util.cc',
'gpio.cc',
'i2c.cc',
'watchdog.cc',
]
_common = fxn('common', common_libs, LIBS="json11")
files = [
'clutil.cc',
'visionimg.cc',
]
if arch == "larch64":
_gpu_libs = ["GLESv2"]
else:
_gpu_libs = ["GL"]
_gpucommon = fxn('gpucommon', files, LIBS=_gpu_libs)
Export('_common', '_gpucommon', '_gpu_libs')
if GetOption('test'):
env.Program('tests/test_util', ['tests/test_util.cc'], LIBS=[_common])
env.Program('tests/test_swaglog', ['tests/test_swaglog.cc'], LIBS=[_common, 'json11', 'zmq', 'pthread'])

@ -23,6 +23,7 @@ from selfdrive.controls.lib.latcontrol_pid import LatControlPID
from selfdrive.controls.lib.latcontrol_indi import LatControlINDI
from selfdrive.controls.lib.latcontrol_angle import LatControlAngle
from selfdrive.controls.lib.latcontrol_torque import LatControlTorque
from selfdrive.controls.lib.latcontrol_lqr import LatControlLQR
from selfdrive.controls.lib.events import Events, ET
from selfdrive.controls.lib.alertmanager import AlertManager, set_offroad_alert
from selfdrive.controls.lib.vehicle_model import VehicleModel
@ -145,6 +146,8 @@ class Controls:
self.LaC = LatControlPID(self.CP, self.CI)
elif self.CP.lateralTuning.which() == 'indi':
self.LaC = LatControlINDI(self.CP, self.CI)
elif self.CP.lateralTuning.which() == 'lqr':
self.LaC = LatControlLQR(self.CP, self.CI)
elif self.CP.lateralTuning.which() == 'torque':
self.LaC = LatControlTorque(self.CP, self.CI)
@ -167,6 +170,8 @@ class Controls:
self.logged_comm_issue = None
self.button_timers = {ButtonEvent.Type.decelCruise: 0, ButtonEvent.Type.accelCruise: 0}
self.last_actuators = car.CarControl.Actuators.new_message()
self.desired_curvature = 0.0
self.desired_curvature_rate = 0.0
# TODO: no longer necessary, aside from process replay
self.sm['liveParameters'].valid = True
@ -440,7 +445,8 @@ class Controls:
# if stock cruise is completely disabled, then we can use our own set speed logic
if not self.CP.pcmCruise:
self.v_cruise_kph = update_v_cruise(self.v_cruise_kph, CS.buttonEvents, self.button_timers, self.enabled, self.is_metric)
self.v_cruise_kph = update_v_cruise(self.v_cruise_kph, CS.vEgo, CS.gasPressed, CS.buttonEvents,
self.button_timers, self.enabled, self.is_metric)
else:
if CS.cruiseState.available:
self.v_cruise_kph = CS.cruiseState.speed * CV.MS_TO_KPH
@ -571,13 +577,13 @@ class Controls:
actuators.accel = self.LoC.update(CC.longActive, CS, long_plan, pid_accel_limits, t_since_plan)
# Steering PID loop and lateral MPC
desired_curvature, desired_curvature_rate = get_lag_adjusted_curvature(self.CP, CS.vEgo,
lat_plan.psis,
lat_plan.curvatures,
lat_plan.curvatureRates)
self.desired_curvature, self.desired_curvature_rate = get_lag_adjusted_curvature(self.CP, CS.vEgo,
lat_plan.psis,
lat_plan.curvatures,
lat_plan.curvatureRates)
actuators.steer, actuators.steeringAngleDeg, lac_log = self.LaC.update(CC.latActive, CS, self.VM, params,
self.last_actuators, desired_curvature,
desired_curvature_rate, self.sm['liveLocationKalman'])
self.last_actuators, self.desired_curvature,
self.desired_curvature_rate, self.sm['liveLocationKalman'])
else:
lac_log = log.ControlsState.LateralDebugState.new_message()
if self.sm.rcv_frame['testJoystick'] > 0:
@ -681,7 +687,7 @@ class Controls:
if self.enabled:
clear_event_types.add(ET.NO_ENTRY)
alerts = self.events.create_alerts(self.current_alert_types, [self.CP, self.sm, self.is_metric, self.soft_disable_timer])
alerts = self.events.create_alerts(self.current_alert_types, [self.CP, CS, self.sm, self.is_metric, self.soft_disable_timer])
self.AM.add_many(self.sm.frame, alerts)
current_alert = self.AM.process_alerts(self.sm.frame, clear_event_types)
if current_alert:
@ -721,6 +727,8 @@ class Controls:
controlsState.enabled = self.enabled
controlsState.active = self.active
controlsState.curvature = curvature
controlsState.desiredCurvature = self.desired_curvature
controlsState.desiredCurvatureRate = self.desired_curvature_rate
controlsState.state = self.state
controlsState.engageable = not self.events.any(ET.NO_ENTRY)
controlsState.longControlState = self.LoC.long_control_state
@ -743,6 +751,8 @@ class Controls:
controlsState.lateralControlState.pidState = lac_log
elif lat_tuning == 'torque':
controlsState.lateralControlState.torqueState = lac_log
elif lat_tuning == 'lqr':
controlsState.lateralControlState.lqrState = lac_log
elif lat_tuning == 'indi':
controlsState.lateralControlState.indiState = lac_log

@ -1,8 +1,9 @@
import math
from cereal import car
from common.conversions import Conversions as CV
from common.numpy_fast import clip, interp
from common.realtime import DT_MDL
from common.conversions import Conversions as CV
from selfdrive.modeld.constants import T_IDXS
# WARNING: this value was determined based on the model's training distribution,
@ -19,14 +20,15 @@ CAR_ROTATION_RADIUS = 0.0
# EU guidelines
MAX_LATERAL_JERK = 5.0
ButtonType = car.CarState.ButtonEvent.Type
CRUISE_LONG_PRESS = 50
CRUISE_NEAREST_FUNC = {
car.CarState.ButtonEvent.Type.accelCruise: math.ceil,
car.CarState.ButtonEvent.Type.decelCruise: math.floor,
ButtonType.accelCruise: math.ceil,
ButtonType.decelCruise: math.floor,
}
CRUISE_INTERVAL_SIGN = {
car.CarState.ButtonEvent.Type.accelCruise: +1,
car.CarState.ButtonEvent.Type.decelCruise: -1,
ButtonType.accelCruise: +1,
ButtonType.decelCruise: -1,
}
@ -40,7 +42,7 @@ def rate_limit(new_value, last_value, dw_step, up_step):
return clip(new_value, last_value + dw_step, last_value + up_step)
def update_v_cruise(v_cruise_kph, buttonEvents, button_timers, enabled, metric):
def update_v_cruise(v_cruise_kph, v_ego, gas_pressed, buttonEvents, button_timers, enabled, metric):
# handle button presses. TODO: this should be in state_control, but a decelCruise press
# would have the effect of both enabling and changing speed is checked after the state transition
if not enabled:
@ -55,7 +57,7 @@ def update_v_cruise(v_cruise_kph, buttonEvents, button_timers, enabled, metric):
for b in buttonEvents:
if b.type.raw in button_timers and not b.pressed:
if button_timers[b.type.raw] > CRUISE_LONG_PRESS:
return v_cruise_kph # end long press
return v_cruise_kph # end long press
button_type = b.type.raw
break
else:
@ -67,10 +69,15 @@ def update_v_cruise(v_cruise_kph, buttonEvents, button_timers, enabled, metric):
if button_type:
v_cruise_delta = v_cruise_delta * (5 if long_press else 1)
if long_press and v_cruise_kph % v_cruise_delta != 0: # partial interval
if long_press and v_cruise_kph % v_cruise_delta != 0: # partial interval
v_cruise_kph = CRUISE_NEAREST_FUNC[button_type](v_cruise_kph / v_cruise_delta) * v_cruise_delta
else:
v_cruise_kph += v_cruise_delta * CRUISE_INTERVAL_SIGN[button_type]
# If set is pressed while overriding, clip cruise speed to minimum of vEgo
if gas_pressed and button_type in (ButtonType.decelCruise, ButtonType.setCruise):
v_cruise_kph = max(v_cruise_kph, v_ego * CV.MS_TO_KPH)
v_cruise_kph = clip(round(v_cruise_kph, 1), V_CRUISE_MIN, V_CRUISE_MAX)
return v_cruise_kph
@ -79,7 +86,7 @@ def update_v_cruise(v_cruise_kph, buttonEvents, button_timers, enabled, metric):
def initialize_v_cruise(v_ego, buttonEvents, v_cruise_last):
for b in buttonEvents:
# 250kph or above probably means we never had a set speed
if b.type in (car.CarState.ButtonEvent.Type.accelCruise, car.CarState.ButtonEvent.Type.resumeCruise) and v_cruise_last < 250:
if b.type in (ButtonType.accelCruise, ButtonType.resumeCruise) and v_cruise_last < 250:
return v_cruise_last
return int(round(clip(v_ego * CV.MS_TO_KPH, V_CRUISE_ENABLE_MIN, V_CRUISE_MAX)))

@ -1,3 +1,4 @@
import math
import os
from enum import IntEnum
from typing import Dict, Union, Callable, List, Optional
@ -141,8 +142,10 @@ class Alert:
class NoEntryAlert(Alert):
def __init__(self, alert_text_2: str, visual_alert: car.CarControl.HUDControl.VisualAlert=VisualAlert.none):
super().__init__("openpilot Unavailable", alert_text_2, AlertStatus.normal,
def __init__(self, alert_text_2: str,
alert_text_1: str = "openpilot Unavailable",
visual_alert: car.CarControl.HUDControl.VisualAlert=VisualAlert.none):
super().__init__(alert_text_1, alert_text_2, AlertStatus.normal,
AlertSize.mid, Priority.LOW, visual_alert,
AudibleAlert.refuse, 3.)
@ -201,35 +204,35 @@ def get_display_speed(speed_ms: float, metric: bool) -> str:
# ********** alert callback functions **********
AlertCallbackType = Callable[[car.CarParams, messaging.SubMaster, bool, int], Alert]
AlertCallbackType = Callable[[car.CarParams, car.CarState, messaging.SubMaster, bool, int], Alert]
def soft_disable_alert(alert_text_2: str) -> AlertCallbackType:
def func(CP: car.CarParams, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert:
def func(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert:
if soft_disable_time < int(0.5 / DT_CTRL):
return ImmediateDisableAlert(alert_text_2)
return SoftDisableAlert(alert_text_2)
return func
def user_soft_disable_alert(alert_text_2: str) -> AlertCallbackType:
def func(CP: car.CarParams, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert:
def func(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert:
if soft_disable_time < int(0.5 / DT_CTRL):
return ImmediateDisableAlert(alert_text_2)
return UserSoftDisableAlert(alert_text_2)
return func
def startup_master_alert(CP: car.CarParams, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert:
def startup_master_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert:
branch = get_short_branch("")
if "REPLAY" in os.environ:
branch = "replay"
return StartupAlert("WARNING: This branch is not tested", branch, alert_status=AlertStatus.userPrompt)
def below_engage_speed_alert(CP: car.CarParams, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert:
def below_engage_speed_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert:
return NoEntryAlert(f"Speed Below {get_display_speed(CP.minEnableSpeed, metric)}")
def below_steer_speed_alert(CP: car.CarParams, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert:
def below_steer_speed_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert:
return Alert(
f"Steer Unavailable Below {get_display_speed(CP.minSteerSpeed, metric)}",
"",
@ -237,7 +240,7 @@ def below_steer_speed_alert(CP: car.CarParams, sm: messaging.SubMaster, metric:
Priority.MID, VisualAlert.steerRequired, AudibleAlert.prompt, 0.4)
def calibration_incomplete_alert(CP: car.CarParams, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert:
def calibration_incomplete_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert:
return Alert(
"Calibration in Progress: %d%%" % sm['liveCalibration'].calPerc,
f"Drive Above {get_display_speed(MIN_SPEED_FILTER, metric)}",
@ -245,7 +248,7 @@ def calibration_incomplete_alert(CP: car.CarParams, sm: messaging.SubMaster, met
Priority.LOWEST, VisualAlert.none, AudibleAlert.none, .2)
def no_gps_alert(CP: car.CarParams, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert:
def no_gps_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert:
gps_integrated = sm['peripheralState'].pandaType in (log.PandaState.PandaType.uno, log.PandaState.PandaType.dos)
return Alert(
"Poor GPS reception",
@ -255,39 +258,72 @@ def no_gps_alert(CP: car.CarParams, sm: messaging.SubMaster, metric: bool, soft_
# *** debug alerts ***
def out_of_space_alert(CP: car.CarParams, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert:
def out_of_space_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert:
full_perc = round(100. - sm['deviceState'].freeSpacePercent)
return NormalPermanentAlert("Out of Storage", f"{full_perc}% full")
def overheat_alert(CP: car.CarParams, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert:
def posenet_invalid_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert:
mdl = sm['modelV2'].velocity.x[0] if len(sm['modelV2'].velocity.x) else math.nan
err = CS.vEgo - mdl
msg = f"Speed Error: {err:.1f} m/s"
return NoEntryAlert(msg, alert_text_1="Posenet Speed Invalid")
def process_not_running_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert:
not_running = [p.name for p in sm['managerState'].processes if not p.running and p.shouldBeRunning]
msg = ', '.join(not_running)
return NoEntryAlert(msg, alert_text_1="Process Not Running")
def comm_issue_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert:
bs = [s for s in sm.data.keys() if not sm.all_checks([s, ])]
msg = ', '.join(bs[:4]) # can't fit too many on one line
return NoEntryAlert(msg, alert_text_1="Communication Issue Between Processes")
def camera_malfunction_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert:
all_cams = ('roadCameraState', 'driverCameraState', 'wideRoadCameraState')
bad_cams = [s for s in all_cams if s in sm.data.keys() and not sm.all_checks([s, ])]
return NormalPermanentAlert("Camera Malfunction", ', '.join(bad_cams))
def calibration_invalid_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert:
rpy = sm['liveCalibration'].rpyCalib
yaw = math.degrees(rpy[2] if len(rpy) == 3 else math.nan)
pitch = math.degrees(rpy[1] if len(rpy) == 3 else math.nan)
angles = f"Pitch: {pitch:.1f}°, Yaw: {yaw:.1f}°"
return NormalPermanentAlert("Calibration Invalid", angles)
def overheat_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert:
cpu = max(sm['deviceState'].cpuTempC, default=0.)
gpu = max(sm['deviceState'].gpuTempC, default=0.)
temp = max((cpu, gpu, sm['deviceState'].memoryTempC))
return NormalPermanentAlert("System Overheated", f"{temp} °C")
return NormalPermanentAlert("System Overheated", f"{temp:.0f} °C")
def low_memory_alert(CP: car.CarParams, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert:
def low_memory_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert:
return NormalPermanentAlert("Low Memory", f"{sm['deviceState'].memoryUsagePercent}% used")
def high_cpu_usage_alert(CP: car.CarParams, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert:
def high_cpu_usage_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert:
x = max(sm['deviceState'].cpuUsagePercent, default=0.)
return NormalPermanentAlert("High CPU Usage", f"{x}% used")
def modeld_lagging_alert(CP: car.CarParams, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert:
return NormalPermanentAlert("Driving model lagging", f"{sm['modelV2'].frameDropPerc}% frames dropped")
def modeld_lagging_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert:
return NormalPermanentAlert("Driving Model Lagging", f"{sm['modelV2'].frameDropPerc:.1f}% frames dropped")
def wrong_car_mode_alert(CP: car.CarParams, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert:
def wrong_car_mode_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert:
text = "Cruise Mode Disabled"
if CP.carName == "honda":
text = "Main Switch Off"
return NoEntryAlert(text)
def joystick_alert(CP: car.CarParams, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert:
def joystick_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert:
axes = sm['testJoystick'].axes
gb, steer = list(axes)[:2] if len(axes) else (0., 0.)
vals = f"Gas: {round(gb * 100.)}%, Steer: {round(steer * 100.)}%"
@ -521,7 +557,7 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = {
# Camera is not outputting frames
EventName.cameraMalfunction: {
ET.PERMANENT: NormalPermanentAlert("Camera Malfunction", "Likely Hardware Issue"),
ET.PERMANENT: camera_malfunction_alert,
},
# Camera framerate too low
EventName.cameraFrameRate: {
@ -653,7 +689,7 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = {
# and attaching while making sure the device is pointed straight forward and is level.
# See https://comma.ai/setup for more information
EventName.calibrationInvalid: {
ET.PERMANENT: NormalPermanentAlert("Calibration Invalid", "Remount Device and Recalibrate"),
ET.PERMANENT: calibration_invalid_alert,
ET.SOFT_DISABLE: soft_disable_alert("Calibration Invalid: Remount Device & Recalibrate"),
ET.NO_ENTRY: NoEntryAlert("Calibration Invalid: Remount Device & Recalibrate"),
},
@ -690,7 +726,7 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = {
# ten times the regular interval, or the average interval is more than 10% too high.
EventName.commIssue: {
ET.SOFT_DISABLE: soft_disable_alert("Communication Issue between Processes"),
ET.NO_ENTRY: NoEntryAlert("Communication Issue between Processes"),
ET.NO_ENTRY: comm_issue_alert,
},
EventName.commIssueAvgFreq: {
ET.SOFT_DISABLE: soft_disable_alert("Low Communication Rate between Processes"),
@ -704,7 +740,7 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = {
# Thrown when manager detects a service exited unexpectedly while driving
EventName.processNotRunning: {
ET.NO_ENTRY: NoEntryAlert("System Malfunction: Reboot Your Device"),
ET.NO_ENTRY: process_not_running_alert,
},
EventName.radarFault: {
@ -716,8 +752,8 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = {
# is not processing frames fast enough they have to be dropped. This alert is
# thrown when over 20% of frames are dropped.
EventName.modeldLagging: {
ET.SOFT_DISABLE: soft_disable_alert("Driving model lagging"),
ET.NO_ENTRY: NoEntryAlert("Driving model lagging"),
ET.SOFT_DISABLE: soft_disable_alert("Driving Model Lagging"),
ET.NO_ENTRY: NoEntryAlert("Driving Model Lagging"),
ET.PERMANENT: modeld_lagging_alert,
},
@ -727,8 +763,8 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = {
# usually means the model has trouble understanding the scene. This is used
# as a heuristic to warn the driver.
EventName.posenetInvalid: {
ET.SOFT_DISABLE: soft_disable_alert("Model Output Uncertain"),
ET.NO_ENTRY: NoEntryAlert("Model Output Uncertain"),
ET.SOFT_DISABLE: soft_disable_alert("Posenet Speed Invalid"),
ET.NO_ENTRY: posenet_invalid_alert,
},
# When the localizer detects an acceleration of more than 40 m/s^2 (~4G) we
@ -852,9 +888,9 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = {
# are received on the car side this usually means the relay hasn't opened correctly
# and this alert is thrown.
EventName.relayMalfunction: {
ET.IMMEDIATE_DISABLE: ImmediateDisableAlert("Harness Malfunction"),
ET.PERMANENT: NormalPermanentAlert("Harness Malfunction", "Check Hardware"),
ET.NO_ENTRY: NoEntryAlert("Harness Malfunction"),
ET.IMMEDIATE_DISABLE: ImmediateDisableAlert("Harness Relay Malfunction"),
ET.PERMANENT: NormalPermanentAlert("Harness Relay Malfunction", "Check Hardware"),
ET.NO_ENTRY: NoEntryAlert("Harness Relay Malfunction"),
},
EventName.noTarget: {

@ -0,0 +1,84 @@
import math
import numpy as np
from common.numpy_fast import clip
from common.realtime import DT_CTRL
from cereal import log
from selfdrive.controls.lib.latcontrol import LatControl, MIN_STEER_SPEED
class LatControlLQR(LatControl):
def __init__(self, CP, CI):
super().__init__(CP, CI)
self.scale = CP.lateralTuning.lqr.scale
self.ki = CP.lateralTuning.lqr.ki
self.A = np.array(CP.lateralTuning.lqr.a).reshape((2, 2))
self.B = np.array(CP.lateralTuning.lqr.b).reshape((2, 1))
self.C = np.array(CP.lateralTuning.lqr.c).reshape((1, 2))
self.K = np.array(CP.lateralTuning.lqr.k).reshape((1, 2))
self.L = np.array(CP.lateralTuning.lqr.l).reshape((2, 1))
self.dc_gain = CP.lateralTuning.lqr.dcGain
self.x_hat = np.array([[0], [0]])
self.i_unwind_rate = 0.3 * DT_CTRL
self.i_rate = 1.0 * DT_CTRL
self.reset()
def reset(self):
super().reset()
self.i_lqr = 0.0
def update(self, active, CS, VM, params, last_actuators, desired_curvature, desired_curvature_rate, llk):
lqr_log = log.ControlsState.LateralLQRState.new_message()
torque_scale = (0.45 + CS.vEgo / 60.0)**2 # Scale actuator model with speed
# Subtract offset. Zero angle should correspond to zero torque
steering_angle_no_offset = CS.steeringAngleDeg - params.angleOffsetAverageDeg
desired_angle = math.degrees(VM.get_steer_from_curvature(-desired_curvature, CS.vEgo, params.roll))
instant_offset = params.angleOffsetDeg - params.angleOffsetAverageDeg
desired_angle += instant_offset # Only add offset that originates from vehicle model errors
lqr_log.steeringAngleDesiredDeg = desired_angle
# Update Kalman filter
angle_steers_k = float(self.C.dot(self.x_hat))
e = steering_angle_no_offset - angle_steers_k
self.x_hat = self.A.dot(self.x_hat) + self.B.dot(CS.steeringTorqueEps / torque_scale) + self.L.dot(e)
if CS.vEgo < MIN_STEER_SPEED or not active:
lqr_log.active = False
lqr_output = 0.
output_steer = 0.
self.reset()
else:
lqr_log.active = True
# LQR
u_lqr = float(desired_angle / self.dc_gain - self.K.dot(self.x_hat))
lqr_output = torque_scale * u_lqr / self.scale
# Integrator
if CS.steeringPressed:
self.i_lqr -= self.i_unwind_rate * float(np.sign(self.i_lqr))
else:
error = desired_angle - angle_steers_k
i = self.i_lqr + self.ki * self.i_rate * error
control = lqr_output + i
if (error >= 0 and (control <= self.steer_max or i < 0.0)) or \
(error <= 0 and (control >= -self.steer_max or i > 0.0)):
self.i_lqr = i
output_steer = lqr_output + self.i_lqr
output_steer = clip(output_steer, -self.steer_max, self.steer_max)
lqr_log.steeringAngleDeg = angle_steers_k
lqr_log.i = self.i_lqr
lqr_log.output = output_steer
lqr_log.lqrOutput = lqr_output
lqr_log.saturated = self._check_saturation(self.steer_max - abs(output_steer) < 1e-3, CS)
return output_steer, desired_angle, lqr_log

@ -25,12 +25,12 @@ JERK_THRESHOLD = 0.2
class LatControlTorque(LatControl):
def __init__(self, CP, CI):
super().__init__(CP, CI)
self.CP = CP
self.pid = PIDController(CP.lateralTuning.torque.kp, CP.lateralTuning.torque.ki,
k_f=CP.lateralTuning.torque.kf, pos_limit=self.steer_max, neg_limit=-self.steer_max)
self.get_steer_feedforward = CI.get_steer_feedforward_function()
self.use_steering_angle = CP.lateralTuning.torque.useSteeringAngle
self.friction = CP.lateralTuning.torque.friction
self.kf = CP.lateralTuning.torque.kf
def reset(self):
super().reset()
@ -42,8 +42,6 @@ class LatControlTorque(LatControl):
if CS.vEgo < MIN_STEER_SPEED or not active:
output_torque = 0.0
pid_log.active = False
if not active:
self.pid.reset()
else:
if self.use_steering_angle:
actual_curvature = -VM.calc_curvature(math.radians(CS.steeringAngleDeg - params.angleOffsetDeg), CS.vEgo, params.roll)
@ -61,11 +59,12 @@ class LatControlTorque(LatControl):
ff = desired_lateral_accel - params.roll * ACCELERATION_DUE_TO_GRAVITY
# convert friction into lateral accel units for feedforward
friction_compensation = interp(desired_lateral_jerk, [-JERK_THRESHOLD, JERK_THRESHOLD], [-self.friction, self.friction])
ff += friction_compensation / self.CP.lateralTuning.torque.kf
ff += friction_compensation / self.kf
freeze_integrator = CS.steeringRateLimited or CS.steeringPressed or CS.vEgo < 5
output_torque = self.pid.update(error,
override=CS.steeringPressed, feedforward=ff,
feedforward=ff,
speed=CS.vEgo,
freeze_integrator=CS.steeringRateLimited)
freeze_integrator=freeze_integrator)
pid_log.active = True
pid_log.p = self.pid.p

@ -9,7 +9,7 @@ from selfdrive.car.honda.values import CAR as HONDA
from selfdrive.car.toyota.values import CAR as TOYOTA
from selfdrive.car.nissan.values import CAR as NISSAN
from selfdrive.controls.lib.latcontrol_pid import LatControlPID
from selfdrive.controls.lib.latcontrol_torque import LatControlTorque
from selfdrive.controls.lib.latcontrol_lqr import LatControlLQR
from selfdrive.controls.lib.latcontrol_indi import LatControlINDI
from selfdrive.controls.lib.latcontrol_angle import LatControlAngle
from selfdrive.controls.lib.vehicle_model import VehicleModel
@ -17,7 +17,7 @@ from selfdrive.controls.lib.vehicle_model import VehicleModel
class TestLatControl(unittest.TestCase):
@parameterized.expand([(HONDA.CIVIC, LatControlPID), (TOYOTA.RAV4, LatControlTorque), (TOYOTA.PRIUS, LatControlINDI), (NISSAN.LEAF, LatControlAngle)])
@parameterized.expand([(HONDA.CIVIC, LatControlPID), (TOYOTA.RAV4, LatControlLQR), (TOYOTA.PRIUS, LatControlINDI), (NISSAN.LEAF, LatControlAngle)])
def test_saturation(self, car_name, controller):
CarInterface, CarController, CarState = interfaces[car_name]
CP = CarInterface.get_params(car_name)

@ -10,6 +10,7 @@ from common.basedir import BASEDIR
from common.params import Params
from selfdrive.controls.lib.events import Alert, EVENTS, ET
from selfdrive.controls.lib.alertmanager import set_offroad_alert
from selfdrive.test.process_replay.process_replay import FakeSubMaster, CONFIGS
AlertSize = log.ControlsState.AlertSize
@ -19,8 +20,7 @@ OFFROAD_ALERTS_PATH = os.path.join(BASEDIR, "selfdrive/controls/lib/alerts_offro
ALERTS = []
for event_types in EVENTS.values():
for alert in event_types.values():
if isinstance(alert, Alert):
ALERTS.append(alert)
ALERTS.append(alert)
class TestAlerts(unittest.TestCase):
@ -30,6 +30,12 @@ class TestAlerts(unittest.TestCase):
with open(OFFROAD_ALERTS_PATH) as f:
cls.offroad_alerts = json.loads(f.read())
# Create fake objects for callback
cls.CS = car.CarState.new_message()
cls.CP = car.CarParams.new_message()
cfg = [c for c in CONFIGS if c.proc_name == 'controlsd'][0]
cls.sm = FakeSubMaster(cfg.pub_sub.keys())
def test_events_defined(self):
# Ensure all events in capnp schema are defined in events.py
events = car.CarEvent.EventName.schema.enumerants
@ -48,7 +54,7 @@ class TestAlerts(unittest.TestCase):
max_text_width = 1920 - 300 # full screen width is useable, minus sidebar
# TODO: get exact scale factor. found this empirically, works well enough
font_scale_factor = 1.85 # factor to scale from nanovg units to PIL
font_scale_factor = 1.55 # factor to scale from nanovg units to PIL
draw = ImageDraw.Draw(Image.new('RGB', (0, 0)))
@ -59,6 +65,9 @@ class TestAlerts(unittest.TestCase):
}
for alert in ALERTS:
if not isinstance(alert, Alert):
alert = alert(self.CP, self.CS, self.sm, metric=False, soft_disable_time=100)
# for full size alerts, both text fields wrap the text,
# so it's unlikely that they would go past the max width
if alert.alert_size in (AlertSize.none, AlertSize.full):

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

Loading…
Cancel
Save