mawei/sunnypilot
1
0
Fork 0
mirror of https://github.com/sunnypilot/sunnypilot.git synced 2026-09-30 21:23:43 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge remote-tracking branch 'commaai/openpilot/master' into palisade-2023-port-1

Browse Source 
# Conflicts:
#	panda
#	selfdrive/car/hyundai/interface.py
This commit is contained in:
Jason Wen
2023-08-18 09:50:44 -04:00
parent ed6e7da39a 0c50dd9d0f
commit 0c2a6d9230
320 changed files with 11540 additions and 13160 deletions
Download Patch File Download Diff File Expand all files Collapse all files
.dockerignore
+3
View File
@@ -16,6 +16,9 @@
*.so
*.a
venv/
.venv/
notebooks
phone
massivemap
.github/workflows/badges.yaml
+1 -1
View File
@@ -7,7 +7,7 @@ on:
env:
BASE_IMAGE: openpilot-base
DOCKER_REGISTRY: ghcr.io/commaai
RUN: docker run --shm-size 1G -v $PWD:/tmp/openpilot -w /tmp/openpilot -e PYTHONPATH=/tmp/openpilot -e NUM_JOBS -e JOB_ID -e GITHUB_ACTION -e GITHUB_REF -e GITHUB_HEAD_REF -e GITHUB_SHA -e GITHUB_REPOSITORY -e GITHUB_RUN_ID -v /tmp/scons_cache:/tmp/scons_cache -v /tmp/comma_download_cache:/tmp/comma_download_cache -v /tmp/openpilot_cache:/tmp/openpilot_cache $DOCKER_REGISTRY/$BASE_IMAGE:latest /bin/sh -c
RUN: docker run --shm-size 1G -v $PWD:/tmp/openpilot -w /tmp/openpilot -e PYTHONPATH=/tmp/openpilot -e NUM_JOBS -e JOB_ID -e GITHUB_ACTION -e GITHUB_REF -e GITHUB_HEAD_REF -e GITHUB_SHA -e GITHUB_REPOSITORY -e GITHUB_RUN_ID -v ~/scons_cache:/tmp/scons_cache -v ~/comma_download_cache:/tmp/comma_download_cache -v ~/openpilot_cache:/tmp/openpilot_cache $DOCKER_REGISTRY/$BASE_IMAGE:latest /bin/sh -c
jobs:
badges:
.github/workflows/selfdrive_tests.yaml
+30 -17
View File
@@ -11,6 +11,8 @@ concurrency:
cancel-in-progress: true
env:
PYTHONWARNINGS: error
BASE_IMAGE: openpilot-base
CL_BASE_IMAGE: openpilot-base-cl
DOCKER_REGISTRY: ghcr.io/commaai
@@ -20,11 +22,11 @@ env:
BUILD: |
DOCKER_BUILDKIT=1 docker build --pull --build-arg BUILDKIT_INLINE_CACHE=1 --cache-from $DOCKER_REGISTRY/$BASE_IMAGE:latest -t $DOCKER_REGISTRY/$BASE_IMAGE:latest -t $BASE_IMAGE:latest -f Dockerfile.openpilot_base .
RUN: docker run --shm-size 1G -v $PWD:/tmp/openpilot -w /tmp/openpilot -e FILEREADER_CACHE=1 -e PYTHONPATH=/tmp/openpilot -e NUM_JOBS -e JOB_ID -e GITHUB_ACTION -e GITHUB_REF -e GITHUB_HEAD_REF -e GITHUB_SHA -e GITHUB_REPOSITORY -e GITHUB_RUN_ID -v /tmp/scons_cache:/tmp/scons_cache -v /tmp/comma_download_cache:/tmp/comma_download_cache -v /tmp/openpilot_cache:/tmp/openpilot_cache $BASE_IMAGE /bin/sh -c
RUN: docker run --shm-size 1G -v $PWD:/tmp/openpilot -w /tmp/openpilot -e PYTHONWARNINGS=error -e FILEREADER_CACHE=1 -e PYTHONPATH=/tmp/openpilot -e NUM_JOBS -e JOB_ID -e GITHUB_ACTION -e GITHUB_REF -e GITHUB_HEAD_REF -e GITHUB_SHA -e GITHUB_REPOSITORY -e GITHUB_RUN_ID -v ~/scons_cache:/tmp/scons_cache -v ~/comma_download_cache:/tmp/comma_download_cache -v ~/openpilot_cache:/tmp/openpilot_cache $BASE_IMAGE /bin/sh -c
BUILD_CL: |
DOCKER_BUILDKIT=1 docker build --build-arg BUILDKIT_INLINE_CACHE=1 --cache-from $DOCKER_REGISTRY/$CL_BASE_IMAGE:latest -t $DOCKER_REGISTRY/$CL_BASE_IMAGE:latest -t $CL_BASE_IMAGE:latest -f Dockerfile.openpilot_base_cl .
RUN_CL: docker run --shm-size 1G -v $PWD:/tmp/openpilot -w /tmp/openpilot -e PYTHONPATH=/tmp/openpilot -e NUM_JOBS -e JOB_ID -e GITHUB_ACTION -e GITHUB_REF -e GITHUB_HEAD_REF -e GITHUB_SHA -e GITHUB_REPOSITORY -e GITHUB_RUN_ID -v /tmp/scons_cache:/tmp/scons_cache -v /tmp/comma_download_cache:/tmp/comma_download_cache -v /tmp/openpilot_cache:/tmp/openpilot_cache $CL_BASE_IMAGE /bin/sh -c
RUN_CL: docker run --shm-size 1G -v $PWD:/tmp/openpilot -w /tmp/openpilot -e PYTHONWARNINGS=error -e PYTHONPATH=/tmp/openpilot -e NUM_JOBS -e JOB_ID -e GITHUB_ACTION -e GITHUB_REF -e GITHUB_HEAD_REF -e GITHUB_SHA -e GITHUB_REPOSITORY -e GITHUB_RUN_ID -v ~/scons_cache:/tmp/scons_cache -v ~/comma_download_cache:/tmp/comma_download_cache -v ~/openpilot_cache:/tmp/openpilot_cache $CL_BASE_IMAGE /bin/sh -c
UNIT_TEST: coverage run --append -m unittest discover
@@ -47,7 +49,7 @@ jobs:
timeout-minutes: 1
run: release/check-submodules.sh
- name: Build openpilot and run checks
timeout-minutes: 10
timeout-minutes: ${{ ((steps.restore-scons-cache.outputs.cache-hit == 'true') && 10 || 30) }} # allow more time when we missed the scons cache
run: |
cd $STRIPPED_DIR
${{ env.RUN }} "CI=1 python selfdrive/manager/build.py"
@@ -66,7 +68,7 @@ jobs:
cp pyproject.toml $STRIPPED_DIR
cp poetry.lock $STRIPPED_DIR
cd $STRIPPED_DIR
${{ env.RUN }} "pre-commit run --all"
${{ env.RUN }} "unset PYTHONWARNINGS && pre-commit run --all"
build_all:
name: build all
@@ -76,16 +78,20 @@ jobs:
with:
submodules: true
- uses: ./.github/workflows/setup
with:
save-cache: true
- name: Build openpilot with all flags
timeout-minutes: 12
timeout-minutes: ${{ ((steps.restore-scons-cache.outputs.cache-hit == 'true') && 12 || 30) }} # allow more time when we missed the scons cache
run: ${{ env.RUN }} "scons -j$(nproc) --extras && release/check-dirty.sh"
- name: Cleanup scons cache
timeout-minutes: 2
- name: Cleanup scons cache and rebuild
timeout-minutes: ${{ ((steps.restore-scons-cache.outputs.cache-hit == 'true') && 2 || 30) }} # allow more time when we missed the scons cache
run: |
${{ env.RUN }} "rm -rf /tmp/scons_cache/* && \
scons -j$(nproc) --cache-populate"
- name: Save scons cache
uses: actions/cache/save@v3
if: github.ref == 'refs/heads/master'
with:
path: ~/scons_cache
key: scons-${{ env.CACHE_COMMIT_DATE }}-${{ github.sha }}
build_mac:
name: build macos
@@ -122,7 +128,7 @@ jobs:
/opt/homebrew/opt
/opt/homebrew/Caskroom/gcc-arm-*
/Applications/ArmGNUToolchain/*/*/*
key: macos_deps-${{ hashFiles('tools/mac_setup.sh', 'update_requirements.sh', 'poetry.lock') }}
key: macos_deps-${{ hashFiles('tools/mac_setup.sh', 'tools/install_python_dependencies.sh', 'poetry.lock') }}
restore-keys: macos_deps-
- name: Brew link restored dependencies
run: |
@@ -138,6 +144,9 @@ jobs:
- name: Install dependencies
if: steps.dependency-cache.outputs.cache-hit != 'true'
run: ./tools/mac_setup.sh
env:
# package install has DeprecationWarnings
PYTHONWARNINGS: default
- name: Build openpilot
run: |
source tools/openpilot_env.sh
@@ -164,8 +173,9 @@ jobs:
if: steps.dependency-cache.outputs.cache-hit != 'true'
run: |
new_cellar=$(brew list --formula -1)
exceptions="zstd lz4 xz" # caching step needs zstd
comm -12 <(echo "$EXISTING_CELLAR") <(echo "$new_cellar") | while read pkg; do
if [[ $pkg != "zstd" ]]; then # caching step needs zstd
if [[ " $exceptions " != *" $pkg "* ]]; then
rm -rf "$(brew --cellar)/$pkg"
fi
done
@@ -210,7 +220,7 @@ jobs:
run: eval "$BUILD"
- name: pre-commit
timeout-minutes: 4
run: ${{ env.RUN }} "pre-commit run --all"
run: ${{ env.RUN }} "unset PYTHONWARNINGS && pre-commit run --all"
valgrind:
name: valgrind
@@ -239,7 +249,7 @@ jobs:
submodules: true
- uses: ./.github/workflows/setup
- name: Build openpilot
timeout-minutes: 10
timeout-minutes: ${{ ((steps.restore-scons-cache.outputs.cache-hit == 'true') && 10 || 30) }} # allow more time when we missed the scons cache
run: ${{ env.RUN }} "scons -j$(nproc)"
- name: Run unit tests
timeout-minutes: 40
@@ -290,7 +300,7 @@ jobs:
id: dependency-cache
uses: actions/cache@v3
with:
path: /tmp/comma_download_cache
path: ~/comma_download_cache
key: proc-replay-${{ hashFiles('.github/workflows/selfdrive_tests.yaml', 'selfdrive/test/process_replay/ref_commit') }}
- name: Build openpilot
run: |
@@ -333,15 +343,18 @@ jobs:
- name: Build openpilot
run: |
${{ env.RUN }} "scons -j$(nproc)"
# PYTHONWARNINGS triggers a SyntaxError in onnxruntime
- name: Run model replay with ONNX
timeout-minutes: 2
run: |
${{ env.RUN_CL }} "ONNXCPU=1 CI=1 NO_NAV=1 coverage run selfdrive/test/process_replay/model_replay.py && \
${{ env.RUN_CL }} "unset PYTHONWARNINGS && \
ONNXCPU=1 CI=1 NO_NAV=1 coverage run selfdrive/test/process_replay/model_replay.py && \
coverage xml"
- name: Run unit tests
timeout-minutes: 4
run: |
${{ env.RUN_CL }} "$UNIT_TEST selfdrive/modeld && \
${{ env.RUN_CL }} "unset PYTHONWARNINGS && \
$UNIT_TEST selfdrive/modeld && \
coverage xml"
- name: "Upload coverage to Codecov"
uses: codecov/codecov-action@v3
@@ -362,7 +375,7 @@ jobs:
id: dependency-cache
uses: actions/cache@v3
with:
path: /tmp/comma_download_cache
path: ~/comma_download_cache
key: car_models-${{ hashFiles('selfdrive/car/tests/test_models.py', 'selfdrive/car/tests/routes.py') }}-${{ matrix.job }}
- name: Build openpilot
run: ${{ env.RUN }} "scons -j$(nproc)"
.github/workflows/setup/action.yaml
+7 -10
View File
@@ -1,15 +1,17 @@
name: 'openpilot env setup'
inputs:
save-cache:
default: false
git_lfs:
description: 'Whether or not to pull the git lfs'
required: false
default: 'true'
runs:
using: "composite"
steps:
# do this after checkout to ensure our custom LFS config is used to pull from GitLab
- shell: bash
if: ${{ inputs.git_lfs == 'true' }}
run: git lfs pull
# build cache
@@ -18,15 +20,10 @@ runs:
run: echo "CACHE_COMMIT_DATE=$(git log -1 --pretty='format:%cd' --date=format:'%Y-%m-%d-%H:%M')" >> $GITHUB_ENV
- shell: bash
run: echo "$CACHE_COMMIT_DATE"
- shell: bash
run: echo "CACHE_SKIP_SAVE=true" >> $GITHUB_ENV
if: github.ref != 'refs/heads/master' || inputs.save-cache == 'false'
- 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
- id: restore-scons-cache
uses: actions/cache/restore@v3
with:
path: /tmp/scons_cache
path: ~/scons_cache
key: scons-${{ env.CACHE_COMMIT_DATE }}-${{ github.sha }}
restore-keys: |
scons-${{ env.CACHE_COMMIT_DATE }}-
.github/workflows/tools_tests.yaml
+12 -10
View File
@@ -19,11 +19,11 @@ env:
BUILD: |
DOCKER_BUILDKIT=1 docker build --pull --build-arg BUILDKIT_INLINE_CACHE=1 --cache-from $DOCKER_REGISTRY/$BASE_IMAGE:latest -t $DOCKER_REGISTRY/$BASE_IMAGE:latest -t $BASE_IMAGE:latest -f Dockerfile.openpilot_base .
RUN: docker run --shm-size 1G -v $GITHUB_WORKSPACE:/tmp/openpilot -w /tmp/openpilot -e FILEREADER_CACHE=1 -e PYTHONPATH=/tmp/openpilot -e NUM_JOBS -e JOB_ID -e GITHUB_ACTION -e GITHUB_REF -e GITHUB_HEAD_REF -e GITHUB_SHA -e GITHUB_REPOSITORY -e GITHUB_RUN_ID -v /tmp/scons_cache:/tmp/scons_cache -v /tmp/comma_download_cache:/tmp/comma_download_cache -v /tmp/openpilot_cache:/tmp/openpilot_cache $BASE_IMAGE /bin/sh -c
RUN: docker run --shm-size 1G -v $GITHUB_WORKSPACE:/tmp/openpilot -w /tmp/openpilot -e FILEREADER_CACHE=1 -e PYTHONPATH=/tmp/openpilot -e NUM_JOBS -e JOB_ID -e GITHUB_ACTION -e GITHUB_REF -e GITHUB_HEAD_REF -e GITHUB_SHA -e GITHUB_REPOSITORY -e GITHUB_RUN_ID -v ~/scons_cache:/tmp/scons_cache -v ~/comma_download_cache:/tmp/comma_download_cache -v ~/openpilot_cache:/tmp/openpilot_cache $BASE_IMAGE /bin/sh -c
BUILD_CL: |
DOCKER_BUILDKIT=1 docker build --pull --build-arg BUILDKIT_INLINE_CACHE=1 --cache-from $DOCKER_REGISTRY/$CL_BASE_IMAGE:latest -t $DOCKER_REGISTRY/$CL_BASE_IMAGE:latest -t $CL_BASE_IMAGE:latest -f Dockerfile.openpilot_base_cl .
RUN_CL: docker run --shm-size 1G -v $GITHUB_WORKSPACE:/tmp/openpilot -w /tmp/openpilot -e PYTHONPATH=/tmp/openpilot -e NUM_JOBS -e JOB_ID -e GITHUB_ACTION -e GITHUB_REF -e GITHUB_HEAD_REF -e GITHUB_SHA -e GITHUB_REPOSITORY -e GITHUB_RUN_ID -v /tmp/scons_cache:/tmp/scons_cache -v /tmp/comma_download_cache:/tmp/comma_download_cache -v /tmp/openpilot_cache:/tmp/openpilot_cache $CL_BASE_IMAGE /bin/sh -c
RUN_CL: docker run --shm-size 1G -v $GITHUB_WORKSPACE:/tmp/openpilot -w /tmp/openpilot -e PYTHONPATH=/tmp/openpilot -e NUM_JOBS -e JOB_ID -e GITHUB_ACTION -e GITHUB_REF -e GITHUB_HEAD_REF -e GITHUB_SHA -e GITHUB_REPOSITORY -e GITHUB_RUN_ID -v ~/scons_cache:/tmp/scons_cache -v ~/comma_download_cache:/tmp/comma_download_cache -v ~/openpilot_cache:/tmp/openpilot_cache $CL_BASE_IMAGE /bin/sh -c
jobs:
@@ -57,10 +57,7 @@ jobs:
- uses: actions/checkout@v3
with:
submodules: true
- name: Pull LFS
run: git lfs pull
- name: Build base image
run: eval "$BUILD"
- uses: ./.github/workflows/setup
- name: Build base cl image
run: eval "$BUILD_CL"
- name: Build simulator image
@@ -75,15 +72,20 @@ jobs:
name: build docs
runs-on: ubuntu-20.04
timeout-minutes: 45
env:
BUILD: |
DOCKER_BUILDKIT=1 docker build --pull --build-arg BUILDKIT_INLINE_CACHE=1 --cache-from $DOCKER_REGISTRY/openpilot-docs-base:latest -t $DOCKER_REGISTRY/openpilot-docs-base:latest -f docs/docker/Dockerfile --target openpilot-docs-base .
DOCKER_BUILDKIT=1 docker build --pull --build-arg BUILDKIT_INLINE_CACHE=1 --cache-from $DOCKER_REGISTRY/openpilot-docs-base:latest --cache-from $DOCKER_REGISTRY/openpilot-docs:latest -t $DOCKER_REGISTRY/openpilot-docs:latest -f docs/docker/Dockerfile .
steps:
- uses: actions/checkout@v3
with:
submodules: true
- name: Build docker container
run: |
DOCKER_BUILDKIT=1 docker build --pull --build-arg BUILDKIT_INLINE_CACHE=1 --cache-from $DOCKER_REGISTRY/openpilot-docs:latest -t $DOCKER_REGISTRY/openpilot-docs:latest -f docs/docker/Dockerfile .
- uses: ./.github/workflows/setup
with:
git_lfs: false
- name: Push docker container
if: github.ref == 'refs/heads/master' && github.event_name != 'pull_request' && github.repository == 'commaai/openpilot'
run: |
$DOCKER_LOGIN
docker push $DOCKER_REGISTRY/openpilot-docs:latest
docker push $DOCKER_REGISTRY/openpilot-docs-base:latest
docker push $DOCKER_REGISTRY/openpilot-docs:latest
.gitignore
+3 -1
View File
@@ -1,10 +1,12 @@
venv/
.venv/
.env
.clang-format
.DS_Store
.tags
.ipynb_checkpoints
.idea
.moc_files
.overlay_init
.overlay_consistent
.sconsign.dblite
@@ -56,7 +58,6 @@ selfdrive/modeld/_dmonitoringmodeld
/src/
one
openpilot
notebooks
xx
yy
@@ -85,3 +86,4 @@ build/
!**/.gitkeep
poetry.toml
Pipfile
.pre-commit-config.yaml
+1 -1
View File
@@ -35,7 +35,7 @@ repos:
args: ['--explicit-package-bases']
exclude: '^(third_party/)|(cereal/)|(opendbc/)|(panda/)|(laika/)|(laika_repo/)|(rednose/)|(rednose_repo/)|(tinygrad/)|(tinygrad_repo/)|(xx/)'
- repo: https://github.com/astral-sh/ruff-pre-commit
rev: v0.0.282
rev: v0.0.284
hooks:
- id: ruff
exclude: '^(third_party/)|(cereal/)|(rednose/)|(panda/)|(laika/)|(laika_repo/)|(rednose_repo/)|(tinygrad/)|(tinygrad_repo/)'
Dockerfile.openpilot_base
+2 -2
View File
@@ -17,8 +17,8 @@ ENV PYENV_VERSION=3.11.4
ENV PYENV_ROOT="/root/.pyenv"
ENV PATH="$PYENV_ROOT/bin:$PYENV_ROOT/shims:$PATH"
COPY pyproject.toml poetry.lock .python-version update_requirements.sh /tmp/
COPY tools/ubuntu_setup.sh /tmp/tools/
COPY pyproject.toml poetry.lock .python-version /tmp/
COPY tools/ubuntu_setup.sh tools/install_python_dependencies.sh /tmp/tools/
RUN cd /tmp && \
tools/ubuntu_setup.sh && \
rm -rf /var/lib/apt/lists/* && \
Jenkinsfile
Vendored
+5 -3
View File
@@ -6,6 +6,7 @@ ssh -tt -o StrictHostKeyChecking=no -i ${key_file} 'comma@${ip}' /usr/bin/bash <
set -e
export CI=1
export PYTHONWARNINGS=error
export LOGPRINT=debug
export TEST_DIR=${env.TEST_DIR}
export SOURCE_DIR=${env.SOURCE_DIR}
@@ -65,6 +66,7 @@ pipeline {
agent none
environment {
CI = "1"
PYTHONWARNINGS = "error"
TEST_DIR = "/data/openpilot"
SOURCE_DIR = "/data/openpilot_source/"
AZURE_TOKEN = credentials('azure_token')
@@ -154,9 +156,10 @@ pipeline {
sh "git config --global --add safe.directory '*'"
sh "git submodule update --init --depth=1 --recursive"
sh "git lfs pull"
sh "scons --clean && scons --no-cache -j42"
// tests that our build system's dependencies are configured properly, needs a machine with lots of cores
sh "scons --clean && scons --no-cache --random -j42"
sh "INTERNAL_SEG_LIST=selfdrive/car/tests/test_models_segs.txt FILEREADER_CACHE=1 pytest -n42 --dist=loadscope selfdrive/car/tests/test_models.py"
sh "INTERNAL_SEG_CNT=500 INTERNAL_SEG_LIST=selfdrive/car/tests/test_models_segs.txt FILEREADER_CACHE=1 \
pytest -n42 --dist=loadscope selfdrive/car/tests/test_models.py"
}
post {
@@ -217,7 +220,6 @@ pipeline {
["build", "cd selfdrive/manager && ./build.py"],
["test pandad", "pytest selfdrive/boardd/tests/test_pandad.py"],
["test power draw", "pytest system/hardware/tici/tests/test_power_draw.py"],
["test loggerd", "pytest system/loggerd/tests/test_loggerd.py"],
["test encoder", "LD_LIBRARY_PATH=/usr/local/lib pytest system/loggerd/tests/test_encoder.py"],
["test pigeond", "pytest system/sensord/tests/test_pigeond.py"],
["test manager", "pytest selfdrive/manager/test/test_manager.py"],
SConstruct
+15 -3
View File
@@ -1,4 +1,5 @@
import os
import shutil
import subprocess
import sys
import sysconfig
@@ -80,7 +81,7 @@ assert arch in ["larch64", "aarch64", "x86_64", "Darwin"]
lenv = {
"PATH": os.environ['PATH'],
"LD_LIBRARY_PATH": [Dir(f"#third_party/acados/{arch}/lib").abspath],
"PYTHONPATH": Dir("#").abspath,
"PYTHONPATH": Dir("#").abspath + ':' + Dir(f"#third_party/acados").abspath,
"ACADOS_SOURCE_DIR": Dir("#third_party/acados").abspath,
"ACADOS_PYTHON_INTERFACE_PATH": Dir("#third_party/acados/acados_template").abspath,
@@ -334,7 +335,18 @@ qt_flags = [
qt_env['CXXFLAGS'] += qt_flags
qt_env['LIBPATH'] += ['#selfdrive/ui']
qt_env['LIBS'] = qt_libs
qt_env['QT_MOCHPREFIX'] = cache_dir + '/moc_files/moc_'
# Have to respect cache-readonly
if GetOption('cache_readonly'):
local_moc_files_dir = Dir("#.moc_files").abspath
cache_moc_files_dir = cache_dir + "/moc_files"
if os.path.exists(local_moc_files_dir):
shutil.rmtree(local_moc_files_dir)
if os.path.exists(cache_moc_files_dir):
shutil.copytree(cache_moc_files_dir, local_moc_files_dir)
qt_env['QT3_MOCHPREFIX'] = local_moc_files_dir + "/moc_"
else:
qt_env['QT3_MOCHPREFIX'] = cache_dir + '/moc_files/moc_'
if GetOption("clazy"):
checks = [
@@ -437,7 +449,7 @@ SConscript(['selfdrive/navd/SConscript'])
SConscript(['selfdrive/modeld/SConscript'])
SConscript(['selfdrive/ui/SConscript'])
if (arch in ['x86_64', 'Darwin'] and Dir('#tools/cabana/').exists()) or GetOption('extras'):
if (arch in ['x86_64', 'aarch64', 'Darwin'] and Dir('#tools/cabana/').exists()) or GetOption('extras'):
SConscript(['tools/replay/SConscript'])
SConscript(['tools/cabana/SConscript'])
cereal
+1 -1
Submodule cereal updated: f01d677e1d...1ee48e0110
common/gpio.py
+4
View File
@@ -1,3 +1,4 @@
import os
from functools import lru_cache
from typing import Optional, List
@@ -26,6 +27,9 @@ def gpio_read(pin: int) -> Optional[bool]:
return val
def gpio_export(pin: int) -> None:
if os.path.isdir(f"/sys/class/gpio/gpio{pin}"):
return
try:
with open("/sys/class/gpio/export", 'w') as f:
f.write(str(pin))
common/spinner.py
+4 -4
View File
@@ -29,11 +29,11 @@ class Spinner():
def close(self):
if self.spinner_proc is not None:
self.spinner_proc.kill()
try:
self.spinner_proc.stdin.close()
except BrokenPipeError:
pass
self.spinner_proc.terminate()
self.spinner_proc.communicate(timeout=2.)
except subprocess.TimeoutExpired:
print("WARNING: failed to kill spinner")
self.spinner_proc = None
def __del__(self):
common/transformations/camera.py
+8 -8
View File
@@ -61,14 +61,6 @@ device_frame_from_view_frame = np.array([
view_frame_from_device_frame = device_frame_from_view_frame.T
def get_calib_from_vp(vp):
vp_norm = normalize(vp)
yaw_calib = np.arctan(vp_norm[0])
pitch_calib = -np.arctan(vp_norm[1]*np.cos(yaw_calib))
roll_calib = 0
return roll_calib, pitch_calib, yaw_calib
# aka 'extrinsic_matrix'
# road : x->forward, y -> left, z->up
def get_view_frame_from_road_frame(roll, pitch, yaw, height):
@@ -131,6 +123,14 @@ def denormalize(img_pts, intrinsics=fcam_intrinsics, width=np.inf, height=np.inf
return img_pts_denormalized[:, :2].reshape(input_shape)
def get_calib_from_vp(vp, intrinsics=fcam_intrinsics):
vp_norm = normalize(vp, intrinsics)
yaw_calib = np.arctan(vp_norm[0])
pitch_calib = -np.arctan(vp_norm[1]*np.cos(yaw_calib))
roll_calib = 0
return roll_calib, pitch_calib, yaw_calib
def device_from_ecef(pos_ecef, orientation_ecef, pt_ecef):
# device from ecef frame
# device frame is x -> forward, y-> right, z -> down
common/util.cc
+22
View File
@@ -2,6 +2,7 @@
#include <sys/ioctl.h>
#include <sys/stat.h>
#include <sys/resource.h>
#include <dirent.h>
#include <cassert>
@@ -60,6 +61,20 @@ int set_core_affinity(std::vector<int> cores) {
#endif
}
int set_file_descriptor_limit(uint64_t limit_val) {
struct rlimit limit;
int status;
if ((status = getrlimit(RLIMIT_NOFILE, &limit)) < 0)
return status;
limit.rlim_cur = limit_val;
if ((status = setrlimit(RLIMIT_NOFILE, &limit)) < 0)
return status;
return 0;
}
std::string read_file(const std::string& fn) {
std::ifstream f(fn, std::ios::binary | std::ios::in);
if (f.is_open()) {
@@ -213,6 +228,13 @@ std::string hexdump(const uint8_t* in, const size_t size) {
return ss.str();
}
int random_int(int min, int max) {
std::random_device dev;
std::mt19937 rng(dev());
std::uniform_int_distribution<std::mt19937::result_type> dist(min, max);
return dist(rng);
}
std::string random_string(std::string::size_type length) {
const std::string chrs = "0123456789abcdefghijklmnopqrstuvwxyzABCDEFGHIJKLMNOPQRSTUVWXYZ";
std::mt19937 rg{std::random_device{}()};
common/util.h
+6 -2
View File
@@ -44,6 +44,7 @@ namespace util {
void set_thread_name(const char* name);
int set_realtime_priority(int level);
int set_core_affinity(std::vector<int> cores);
int set_file_descriptor_limit(uint64_t limit);
// ***** Time helpers *****
struct tm get_time();
@@ -75,10 +76,13 @@ int getenv(const char* key, int default_val);
float getenv(const char* key, float default_val);
std::string hexdump(const uint8_t* in, const size_t size);
std::string random_string(std::string::size_type length);
std::string dir_name(std::string const& path);
// **** file fhelpers *****
// ***** random helpers *****
int random_int(int min, int max);
std::string random_string(std::string::size_type length);
// **** file helpers *****
std::string read_file(const std::string& fn);
std::map<std::string, std::string> read_files_in_dir(const std::string& path);
int write_file(const char* path, const void* data, size_t size, int flags = O_WRONLY, mode_t mode = 0664);
docs/conf.py
+1 -1
View File
@@ -100,7 +100,7 @@ breathe_projects_source = {}
# only document files that have accompanying .cc files next to them
print("searching for c_docs...")
for root, dirs, files in os.walk(BASEDIR):
for root, _, files in os.walk(BASEDIR):
found = False
breath_src = {}
breathe_srcs_list = []
docs/docker/Dockerfile
+1 -1
View File
@@ -1,4 +1,4 @@
FROM ghcr.io/commaai/openpilot-base:latest
FROM ghcr.io/commaai/openpilot-base:latest as openpilot-docs-base
ENV PYTHONUNBUFFERED 1
openpilot/__init__.py
+3
View File
@@ -0,0 +1,3 @@
openpilot/common
Symlink
+1
View File
@@ -0,0 +1 @@
../common
openpilot/selfdrive
Symlink
+1
View File
@@ -0,0 +1 @@
../selfdrive/
openpilot/system
Symlink
+1
View File
@@ -0,0 +1 @@
../system/
openpilot/third_party
Symlink
+1
View File
@@ -0,0 +1 @@
../third_party
openpilot/tools
Symlink
+1
View File
@@ -0,0 +1 @@
../tools
panda
+1 -1
Submodule panda updated: a09ca7de34...c15f8ff6ad
poetry.lock
Generated
+236 -6273
View File
File diff suppressed because it is too large Load Diff
pyproject.toml
+7 -94
View File
@@ -17,7 +17,6 @@ documentation = "https://docs.comma.ai"
[tool.poetry.dependencies]
python = "~3.11"
poetry = "~1.5.1"
atomicwrites = "*"
aiohttp = "*"
aiortc = "*"
@@ -26,19 +25,16 @@ cffi = "*"
crcmod = "*"
cryptography = "*"
Cython = "*"
Flask = "*"
future-fstrings = "*" # for acados
gunicorn = "*"
hatanaka = "==2.4"
hexdump = "*"
Jinja2 = "*"
json-rpc = "*"
libusb1 = "*"
numpy = "==1.23.0" # locked pending deprecation fixes in xx
numpy = "*"
onnx = ">=1.14.0"
onnxruntime-gpu = { version = ">=1.15.1", platform = "linux", markers = "platform_machine == 'x86_64'" }
pillow = "*"
protobuf = "==3.20.3"
psutil = "*"
pyaudio = "*"
pycapnp = "*"
@@ -47,12 +43,11 @@ pydub = "*"
PyJWT = "*"
pyopencl = "*"
pyserial = "*"
python-dateutil = "*"
PyYAML = "*"
pyzmq = "*"
requests = "*"
scons = "*"
sentry-sdk = "~1.28"
sentry-sdk = "==1.28.1" # needs to be updated with AGNOS
setproctitle = "*"
smbus2 = "*"
sounddevice = "*"
@@ -62,7 +57,6 @@ sympy = "*"
timezonefinder = "*"
tqdm = "*"
urllib3 = "*"
utm = "*"
websocket_client = "*"
polyline = "*"
sconscontrib = {git = "https://github.com/SCons/scons-contrib.git"}
@@ -73,26 +67,21 @@ av = "*"
azure-storage-blob = "~2.1"
breathe = "*"
carla = { url = "https://github.com/commaai/carla/releases/download/3.11.4/carla-0.9.14-cp311-cp311-linux_x86_64.whl", platform = "linux", markers = "platform_machine == 'x86_64'" }
control = "*"
coverage = "*"
dictdiffer = "*"
fastcluster = "*"
ft4222 = "*"
hexdump = "*"
hypothesis = "==6.46.7"
inputs = "*"
lru-dict = "*"
lxml = "*"
markdown-it-py = "*"
matplotlib = "*"
mpld3 = "*"
mypy = "*"
myst-parser = "*"
natsort = "*"
opencv-python-headless = { url = "https://github.com/commaai/opencv-python-builder/releases/download/4.5.5.64%2Bcu118-cp311/opencv_python_headless-4.5.5.64-cp311-cp311-manylinux_2_31_x86_64.whl", platform = "linux" }
opencv-python-headless = "*"
pandas = "*"
parameterized = "^0.8"
paramiko = "*"
pprofile = "*"
pre-commit = "*"
pycurl = "*"
@@ -100,8 +89,7 @@ pygame = "*"
pyprof2calltree = "*"
pytest = "*"
pytest-xdist = "*"
reverse_geocoder = "*"
scipy = "==1.9.3" # pinned until xx refs changes can be checked
scipy = "*"
sphinx = "*"
sphinx-rtd-theme = "*"
sphinx-sitemap = "*"
@@ -110,85 +98,10 @@ tenacity = "*"
types-atomicwrites = "*"
types-certifi = "*"
types-pycurl = "*"
types-python-dateutil = "*"
types-PyYAML = "*"
types-requests = "*"
types-tabulate = "*"
[tool.poetry.group.xx]
optional = true
[tool.poetry.group.xx.dependencies]
aenum = "*"
aiohttp = "*"
albumentations = "*"
azure-cli-core = "*"
azure-common = "*"
azure-core = "*"
azure-nspkg = "~3.0"
azure-storage-common = "~2.1"
azure-storage-nspkg = "~3.1"
blosc = "==1.9.2"
cloudpickle = "*"
configargparse = "*"
cupy-cuda11x = "*"
datadog = "*"
dotmap = "*"
einops = "*"
elasticsearch = "*"
flake8 = "*"
Flask-Cors = "*"
Flask-SocketIO = "*"
GeoAlchemy2 = "*"
imageio = "*"
influxdb-client = "*"
ipykernel = "*"
ipython = "*"
joblib = "*"
json-logging-py = "*"
jupyter = "*"
jupyterlab = "*"
jupyterlab-vim = "*"
Markdown = "*"
msgpack-python = "*"
networkx = "~2.8"
nvidia-ml-py3 = "*"
onnx2torch = "*"
onnxoptimizer = "*"
osmium = "*"
pillow-avif-plugin = "*"
pipenv = "==2022.10.12"
plotly = "*"
pycuda = "*"
Pygments = "*"
pylint = "*"
PyMySQL = "~0.9"
pyproj = "*"
python-logstash = "*"
redis = "*"
ruff = "*"
s2sphere = "*"
scikit-image = "*"
scikit-learn = "*"
segmentation-models-pytorch = "==0.3.3"
simplejson = "*"
SQLAlchemy = "*"
wrapt = "*"
torch = { url = "https://download.pytorch.org/whl/cu118/torch-2.0.1%2Bcu118-cp311-cp311-linux_x86_64.whl" }
torchsummary = "*"
torchvision = { url = "https://download.pytorch.org/whl/cu118/torchvision-0.15.2%2Bcu118-cp311-cp311-linux_x86_64.whl" }
triton = "*"
Werkzeug = "*"
zerorpc = { git = "https://github.com/commaai/zerorpc-python.git", branch = "master" }
omegaconf = "*"
osmnx = "==1.2.2"
tritonclient = {version = "2.28.0", extras = ["http"]}
transformers = "*"
timm = "==0.9.2"
PyNvCodec = { git = "https://github.com/NVIDIA/VideoProcessingFramework.git" }
apex = { url = "https://github.com/commaai/apex/releases/download/pytorch2.0.1%2Bcu11.8/apex-0.1-cp311-cp311-linux_x86_64.whl" }
opencv-python-headless = { url = "https://github.com/commaai/opencv-python-builder/releases/download/4.5.5.64%2Bcu118-cp311/opencv_python_headless-4.5.5.64-cp311-cp311-manylinux_2_31_x86_64.whl", platform = "linux" }
pyqt5 = { version = "*", markers = "platform_machine == 'x86_64'" } # no aarch64 wheels for macOS/linux
[build-system]
@@ -197,8 +110,8 @@ build-backend = "poetry.core.masonry.api"
# https://beta.ruff.rs/docs/configuration/#using-pyprojecttoml
[tool.ruff]
select = ["E", "F", "W", "PIE", "C4", "ISC", "RUF100", "A"]
ignore = ["W292", "E741", "E402", "C408", "ISC003"]
select = ["E", "F", "W", "PIE", "C4", "ISC", "RUF100", "A", "B"]
ignore = ["W292", "E741", "E402", "C408", "ISC003", "B027", "B024"]
line-length = 160
target-version="py311"
exclude = [
release/files_common
-2
View File
@@ -418,8 +418,6 @@ selfdrive/assets/navigation/*
third_party/.gitignore
third_party/SConscript
third_party/cluster/*
third_party/linux/**
third_party/opencl/**
scripts/code_stats.py
+1 -1
View File
@@ -8,7 +8,7 @@ fouts = {x.decode('utf-8') for x in subprocess.check_output(['git', 'ls-files'])
pyf = []
for d in ["cereal", "common", "scripts", "selfdrive", "tools"]:
for root, dirs, files in os.walk(d):
for root, _, files in os.walk(d):
for f in files:
if f.endswith(".py"):
pyf.append(os.path.join(root, f))
selfdrive/assets/navigation/direction_turn_slight_left_inactive.png
BIN
View File
Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 3.6 KiB

selfdrive/assets/navigation/direction_turn_slight_right_inactive.png
BIN
View File
Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 3.7 KiB

selfdrive/athena/athenad.py
+11 -10
View File
@@ -213,6 +213,16 @@ def retry_upload(tid: int, end_event: threading.Event, increase_count: bool = Tr
break
def cb(sm, item, tid, sz: int, cur: int) -> None:
# Abort transfer if connection changed to metered after starting upload
sm.update(0)
metered = sm['deviceState'].networkMetered
if metered and (not item.allow_cellular):
raise AbortTransferException
cur_upload_items[tid] = replace(item, progress=cur / sz if sz else 1)
def upload_handler(end_event: threading.Event) -> None:
sm = messaging.SubMaster(['deviceState'])
tid = threading.get_ident()
@@ -242,15 +252,6 @@ def upload_handler(end_event: threading.Event) -> None:
continue
try:
def cb(sz: int, cur: int) -> None:
# Abort transfer if connection changed to metered after starting upload
sm.update(0)
metered = sm['deviceState'].networkMetered
if metered and (not item.allow_cellular):
raise AbortTransferException
cur_upload_items[tid] = replace(item, progress=cur / sz if sz else 1)
fn = item.path
try:
sz = os.path.getsize(fn)
@@ -258,7 +259,7 @@ def upload_handler(end_event: threading.Event) -> None:
sz = -1
cloudlog.event("athena.upload_handler.upload_start", fn=fn, sz=sz, network_type=network_type, metered=metered, retry_count=item.retry_count)
response = _do_upload(item, cb)
response = _do_upload(item, partial(cb, sm, item, tid))
if response.status_code not in (200, 201, 401, 403, 412):
cloudlog.event("athena.upload_handler.retry", status_code=response.status_code, fn=fn, sz=sz, network_type=network_type, metered=metered)
selfdrive/boardd/tests/test_boardd_loopback.py
-7
View File
@@ -10,7 +10,6 @@ from pprint import pprint
import cereal.messaging as messaging
from cereal import car, log
from common.params import Params
from common.spinner import Spinner
from common.timeout import Timeout
from selfdrive.boardd.boardd import can_list_to_can_capnp
from selfdrive.car import make_can_msg
@@ -24,11 +23,6 @@ class TestBoardd(unittest.TestCase):
def setUpClass(cls):
os.environ['STARTED'] = '1'
os.environ['BOARDD_LOOPBACK'] = '1'
cls.spinner = Spinner()
@classmethod
def tearDownClass(cls):
cls.spinner.close()
@phone_only
@with_processes(['pandad'])
@@ -67,7 +61,6 @@ class TestBoardd(unittest.TestCase):
n = 200
for i in range(n):
print(f"boardd loopback {i}/{n}")
self.spinner.update(f"boardd loopback {i}/{n}")
sent_msgs = defaultdict(set)
for _ in range(random.randrange(20, 100)):
selfdrive/boardd/tests/test_boardd_usbprotocol.cc
+3 -10
View File
@@ -1,18 +1,11 @@
#define CATCH_CONFIG_MAIN
#define CATCH_CONFIG_ENABLE_BENCHMARKING
#include <random>
#include "catch2/catch.hpp"
#include "cereal/messaging/messaging.h"
#include "common/util.h"
#include "selfdrive/boardd/panda.h"
int random_int(int min, int max) {
std::random_device dev;
std::mt19937 rng(dev());
std::uniform_int_distribution<std::mt19937::result_type> dist(min, max);
return dist(rng);
}
struct PandaTest : public Panda {
PandaTest(uint32_t bus_offset, int can_list_size, cereal::PandaState::PandaType hw_type);
void test_can_send();
@@ -44,10 +37,10 @@ PandaTest::PandaTest(uint32_t bus_offset_, int can_list_size, cereal::PandaState
auto can_list = msg.initEvent().initSendcan(can_list_size);
for (uint8_t i = 0; i < can_list_size; ++i) {
auto can = can_list[i];
uint32_t id = random_int(0, std::size(dlc_to_len) - 1);
uint32_t id = util::random_int(0, std::size(dlc_to_len) - 1);
const std::string &dat = test_data[dlc_to_len[id]];
can.setAddress(i);
can.setSrc(random_int(0, 3) + bus_offset);
can.setSrc(util::random_int(0, 3) + bus_offset);
can.setDat(kj::ArrayPtr((uint8_t *)dat.data(), dat.size()));
total_pakets_size += sizeof(can_header) + dat.size();
}
selfdrive/car/__init__.py
+29 -9
View File
@@ -40,9 +40,8 @@ def gen_empty_fingerprint():
return {i: {} for i in range(0, 8)}
# FIXME: hardcoding honda civic 2016 touring params so they can be used to
# scale unknown params for other cars
class CivicParams:
# these params were derived for the Civic and used to calculate params for other cars
class VehicleDynamicsParams:
MASS = 1326. + STD_CARGO_KG
WHEELBASE = 2.70
CENTER_TO_FRONT = WHEELBASE * 0.4
@@ -55,18 +54,18 @@ class CivicParams:
# TODO: get actual value, for now starting with reasonable value for
# civic and scaling by mass and wheelbase
def scale_rot_inertia(mass, wheelbase):
return CivicParams.ROTATIONAL_INERTIA * mass * wheelbase ** 2 / (CivicParams.MASS * CivicParams.WHEELBASE ** 2)
return VehicleDynamicsParams.ROTATIONAL_INERTIA * mass * wheelbase ** 2 / (VehicleDynamicsParams.MASS * VehicleDynamicsParams.WHEELBASE ** 2)
# TODO: start from empirically derived lateral slip stiffness for the civic and scale by
# mass and CG position, so all cars will have approximately similar dyn behaviors
def scale_tire_stiffness(mass, wheelbase, center_to_front, tire_stiffness_factor=1.0):
def scale_tire_stiffness(mass, wheelbase, center_to_front, tire_stiffness_factor):
center_to_rear = wheelbase - center_to_front
tire_stiffness_front = (CivicParams.TIRE_STIFFNESS_FRONT * tire_stiffness_factor) * mass / CivicParams.MASS * \
(center_to_rear / wheelbase) / (CivicParams.CENTER_TO_REAR / CivicParams.WHEELBASE)
tire_stiffness_front = (VehicleDynamicsParams.TIRE_STIFFNESS_FRONT * tire_stiffness_factor) * mass / VehicleDynamicsParams.MASS * \
(center_to_rear / wheelbase) / (VehicleDynamicsParams.CENTER_TO_REAR / VehicleDynamicsParams.WHEELBASE)
tire_stiffness_rear = (CivicParams.TIRE_STIFFNESS_REAR * tire_stiffness_factor) * mass / CivicParams.MASS * \
(center_to_front / wheelbase) / (CivicParams.CENTER_TO_FRONT / CivicParams.WHEELBASE)
tire_stiffness_rear = (VehicleDynamicsParams.TIRE_STIFFNESS_REAR * tire_stiffness_factor) * mass / VehicleDynamicsParams.MASS * \
(center_to_front / wheelbase) / (VehicleDynamicsParams.CENTER_TO_FRONT / VehicleDynamicsParams.WHEELBASE)
return tire_stiffness_front, tire_stiffness_rear
@@ -213,3 +212,24 @@ class CanBusBase:
else:
num = len(CP.safetyConfigs)
self.offset = 4 * (num - 1)
class CanSignalRateCalculator:
"""
Calculates the instantaneous rate of a CAN signal by using the counter
variable and the known frequency of the CAN message that contains it.
"""
def __init__(self, frequency):
self.frequency = frequency
self.previous_counter = 0
self.previous_value = 0
self.rate = 0
def update(self, current_value, current_counter):
if current_counter != self.previous_counter:
self.rate = (current_value - self.previous_value) * self.frequency
self.previous_counter = current_counter
self.previous_value = current_value
return self.rate
selfdrive/car/car_helpers.py
+46 -39
View File
@@ -1,5 +1,5 @@
import os
from typing import Dict, List
from typing import Callable, Dict, List, Optional, Tuple
from cereal import car
from common.params import Params
@@ -13,6 +13,8 @@ from system.swaglog import cloudlog
import cereal.messaging as messaging
from selfdrive.car import gen_empty_fingerprint
FRAME_FINGERPRINT = 100 # 1s
EventName = car.CarEvent.EventName
@@ -75,6 +77,46 @@ interface_names = _get_interface_names()
interfaces = load_interfaces(interface_names)
def can_fingerprint(next_can: Callable) -> Tuple[Optional[str], Dict[int, dict]]:
finger = gen_empty_fingerprint()
candidate_cars = {i: all_legacy_fingerprint_cars() for i in [0, 1]} # attempt fingerprint on both bus 0 and 1
frame = 0
car_fingerprint = None
done = False
while not done:
a = next_can()
for can in a.can:
# The fingerprint dict is generated for all buses, this way the car interface
# can use it to detect a (valid) multipanda setup and initialize accordingly
if can.src < 128:
if can.src not in finger:
finger[can.src] = {}
finger[can.src][can.address] = len(can.dat)
for b in candidate_cars:
# Ignore extended messages and VIN query response.
if can.src == b and can.address < 0x800 and can.address not in (0x7df, 0x7e0, 0x7e8):
candidate_cars[b] = eliminate_incompatible_cars(can, candidate_cars[b])
# if we only have one car choice and the time since we got our first
# message has elapsed, exit
for b in candidate_cars:
if len(candidate_cars[b]) == 1 and frame > FRAME_FINGERPRINT:
# fingerprint done
car_fingerprint = candidate_cars[b][0]
# bail if no cars left or we've been waiting for more than 2s
failed = (all(len(cc) == 0 for cc in candidate_cars.values()) and frame > FRAME_FINGERPRINT) or frame > 200
succeeded = car_fingerprint is not None
done = failed or succeeded
frame += 1
return car_fingerprint, finger
# **** for use live only ****
def fingerprint(logcan, sendcan, num_pandas):
fixed_fingerprint = os.environ.get('FINGERPRINT', "")
@@ -123,45 +165,10 @@ def fingerprint(logcan, sendcan, num_pandas):
set_obd_multiplexing(params, False)
params.put_bool("FirmwareQueryDone", True)
finger = gen_empty_fingerprint()
candidate_cars = {i: all_legacy_fingerprint_cars() for i in [0, 1]} # attempt fingerprint on both bus 0 and 1
frame = 0
frame_fingerprint = 100 # 1s
car_fingerprint = None
done = False
# drain CAN socket so we always get the latest messages
# CAN fingerprint
# drain CAN socket so we get the latest messages
messaging.drain_sock_raw(logcan)
while not done:
a = get_one_can(logcan)
for can in a.can:
# The fingerprint dict is generated for all buses, this way the car interface
# can use it to detect a (valid) multipanda setup and initialize accordingly
if can.src < 128:
if can.src not in finger:
finger[can.src] = {}
finger[can.src][can.address] = len(can.dat)
for b in candidate_cars:
# Ignore extended messages and VIN query response.
if can.src == b and can.address < 0x800 and can.address not in (0x7df, 0x7e0, 0x7e8):
candidate_cars[b] = eliminate_incompatible_cars(can, candidate_cars[b])
# if we only have one car choice and the time since we got our first
# message has elapsed, exit
for b in candidate_cars:
if len(candidate_cars[b]) == 1 and frame > frame_fingerprint:
# fingerprint done
car_fingerprint = candidate_cars[b][0]
# bail if no cars left or we've been waiting for more than 2s
failed = (all(len(cc) == 0 for cc in candidate_cars.values()) and frame > frame_fingerprint) or frame > 200
succeeded = car_fingerprint is not None
done = failed or succeeded
frame += 1
car_fingerprint, finger = can_fingerprint(lambda: get_one_can(logcan))
exact_match = True
source = car.CarParams.FingerprintSource.can
selfdrive/car/chrysler/interface.py
+5 -5
View File
@@ -1,7 +1,7 @@
#!/usr/bin/env python3
from cereal import car
from panda import Panda
from selfdrive.car import STD_CARGO_KG, get_safety_config
from selfdrive.car import get_safety_config
from selfdrive.car.chrysler.values import CAR, RAM_HD, RAM_DT, RAM_CARS, ChryslerFlags
from selfdrive.car.interfaces import CarInterfaceBase
@@ -35,7 +35,7 @@ class CarInterface(CarInterfaceBase):
# Chrysler
if candidate in (CAR.PACIFICA_2017_HYBRID, CAR.PACIFICA_2018, CAR.PACIFICA_2018_HYBRID, CAR.PACIFICA_2019_HYBRID, CAR.PACIFICA_2020):
ret.mass = 2242. + STD_CARGO_KG
ret.mass = 2242.
ret.wheelbase = 3.089
ret.steerRatio = 16.2 # Pacifica Hybrid 2017
@@ -46,7 +46,7 @@ class CarInterface(CarInterfaceBase):
# Jeep
elif candidate in (CAR.JEEP_CHEROKEE, CAR.JEEP_CHEROKEE_2019):
ret.mass = 1778 + STD_CARGO_KG
ret.mass = 1778
ret.wheelbase = 2.71
ret.steerRatio = 16.7
ret.steerActuatorDelay = 0.2
@@ -61,7 +61,7 @@ class CarInterface(CarInterfaceBase):
ret.steerActuatorDelay = 0.2
ret.wheelbase = 3.88
ret.steerRatio = 16.3
ret.mass = 2493. + STD_CARGO_KG
ret.mass = 2493.
ret.minSteerSpeed = 14.5
# Older EPS FW allow steer to zero
if any(fw.ecu == 'eps' and fw.fwVersion[:4] <= b"6831" for fw in car_fw):
@@ -71,7 +71,7 @@ class CarInterface(CarInterfaceBase):
ret.steerActuatorDelay = 0.2
ret.wheelbase = 3.785
ret.steerRatio = 15.61
ret.mass = 3405. + STD_CARGO_KG
ret.mass = 3405.
ret.minSteerSpeed = 16
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning, 1.0, False)
selfdrive/car/chrysler/radar_interface.py
+2 -2
View File
@@ -25,7 +25,7 @@ def _create_radar_can_parser(car_fingerprint):
messages = list(zip(RADAR_MSGS_C +
RADAR_MSGS_D,
[20] * msg_n + # 20Hz (0.05s)
[20] * msg_n)) # 20Hz (0.05s)
[20] * msg_n, strict=True)) # 20Hz (0.05s)
return CANParser(DBC[car_fingerprint]['radar'], messages, 1)
@@ -46,7 +46,7 @@ class RadarInterface(RadarInterfaceBase):
def update(self, can_strings):
if self.rcp is None or self.CP.radarUnavailable:
return super().update(None)
return None
vls = self.rcp.update_strings(can_strings)
self.updated_messages.update(vls)
selfdrive/car/ecu_addrs.py
+3 -4
View File
@@ -1,16 +1,15 @@
#!/usr/bin/env python3
import capnp
import time
from typing import Optional, Set, Tuple
from typing import Optional, Set
import cereal.messaging as messaging
from panda.python.uds import SERVICE_TYPE
from selfdrive.car import make_can_msg
from selfdrive.car.fw_query_definitions import EcuAddrBusType
from selfdrive.boardd.boardd import can_list_to_can_capnp
from system.swaglog import cloudlog
EcuAddrBusType = Tuple[int, Optional[int], int]
def make_tester_present_msg(addr, bus, subaddr=None):
dat = [0x02, SERVICE_TYPE.TESTER_PRESENT, 0x0]
@@ -90,7 +89,7 @@ if __name__ == "__main__":
print()
print("Found ECUs on addresses:")
for addr, subaddr, bus in ecu_addrs:
for addr, subaddr, _ in ecu_addrs:
msg = f" 0x{hex(addr)}"
if subaddr is not None:
msg += f" (sub-address: {hex(subaddr)})"
selfdrive/car/fingerprints.py
+2 -3
View File
@@ -29,9 +29,8 @@ def eliminate_incompatible_cars(msg, candidate_cars):
car_fingerprints = _FINGERPRINTS[car_name]
for fingerprint in car_fingerprints:
fingerprint.update(_DEBUG_ADDRESS) # add alien debug address
if is_valid_for_fingerprint(msg, fingerprint):
# add alien debug address
if is_valid_for_fingerprint(msg, fingerprint | _DEBUG_ADDRESS):
compatible_cars.append(car_name)
break
selfdrive/car/ford/interface.py
+7 -7
View File
@@ -2,7 +2,7 @@
from cereal import car
from panda import Panda
from common.conversions import Conversions as CV
from selfdrive.car import STD_CARGO_KG, get_safety_config
from selfdrive.car import get_safety_config
from selfdrive.car.ford.fordcan import CanBus
from selfdrive.car.ford.values import CANFD_CAR, CAR, Ecu
from selfdrive.car.interfaces import CarInterfaceBase
@@ -39,33 +39,33 @@ class CarInterface(CarInterfaceBase):
if candidate == CAR.BRONCO_SPORT_MK1:
ret.wheelbase = 2.67
ret.steerRatio = 17.7
ret.mass = 1625 + STD_CARGO_KG
ret.mass = 1625
elif candidate == CAR.ESCAPE_MK4:
ret.wheelbase = 2.71
ret.steerRatio = 16.7
ret.mass = 1750 + STD_CARGO_KG
ret.mass = 1750
elif candidate == CAR.EXPLORER_MK6:
ret.wheelbase = 3.025
ret.steerRatio = 16.8
ret.mass = 2050 + STD_CARGO_KG
ret.mass = 2050
elif candidate == CAR.F_150_MK14:
# required trim only on SuperCrew
ret.wheelbase = 3.69
ret.steerRatio = 17.0
ret.mass = 2000 + STD_CARGO_KG
ret.mass = 2000
elif candidate == CAR.FOCUS_MK4:
ret.wheelbase = 2.7
ret.steerRatio = 15.0
ret.mass = 1350 + STD_CARGO_KG
ret.mass = 1350
elif candidate == CAR.MAVERICK_MK1:
ret.wheelbase = 3.076
ret.steerRatio = 17.0
ret.mass = 1650 + STD_CARGO_KG
ret.mass = 1650
else:
raise ValueError(f"Unsupported car: {candidate}")
selfdrive/car/ford/radar_interface.py
+2 -2
View File
@@ -15,7 +15,7 @@ DELPHI_MRR_RADAR_MSG_COUNT = 64
def _create_delphi_esr_radar_can_parser(CP) -> CANParser:
msg_n = len(DELPHI_ESR_RADAR_MSGS)
messages = list(zip(DELPHI_ESR_RADAR_MSGS, [20] * msg_n))
messages = list(zip(DELPHI_ESR_RADAR_MSGS, [20] * msg_n, strict=True))
return CANParser(RADAR.DELPHI_ESR, messages, CanBus(CP).radar)
@@ -51,7 +51,7 @@ class RadarInterface(RadarInterfaceBase):
def update(self, can_strings):
if self.rcp is None:
return super().update(None)
return None
vls = self.rcp.update_strings(can_strings)
self.updated_messages.update(vls)
selfdrive/car/ford/values.py
+4
View File
@@ -156,6 +156,7 @@ FW_VERSIONS = {
b'LX6C-2D053-NT\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'LX6C-2D053-NY\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'LX6C-2D053-SA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'LX6C-2D053-SD\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.fwdRadar, 0x764, None): [
b'LB5T-14D049-AB\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
@@ -163,11 +164,13 @@ FW_VERSIONS = {
(Ecu.fwdCamera, 0x706, None): [
b'LJ6T-14F397-AD\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'LJ6T-14F397-AE\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'LV4T-14F397-GG\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.engine, 0x7E0, None): [
b'LX6A-14C204-BJV\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'LX6A-14C204-BJX\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'LX6A-14C204-CNG\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'LX6A-14C204-DPK\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'LX6A-14C204-ESG\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'MX6A-14C204-BEF\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'MX6A-14C204-BEJ\x00\x00\x00\x00\x00\x00\x00\x00\x00',
@@ -201,6 +204,7 @@ FW_VERSIONS = {
],
(Ecu.engine, 0x7E0, None): [
b'LB5A-14C204-ATJ\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'LB5A-14C204-AUJ\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'LB5A-14C204-AZL\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'LB5A-14C204-BUJ\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'LB5A-14C204-EAC\x00\x00\x00\x00\x00\x00\x00\x00\x00',
selfdrive/car/fw_query_definitions.py
+4 -1
View File
@@ -7,6 +7,9 @@ from typing import Callable, Dict, List, Optional, Set, Tuple
import panda.python.uds as uds
AddrType = Tuple[int, Optional[int]]
EcuAddrBusType = Tuple[int, Optional[int], int]
def p16(val):
return struct.pack("!H", val)
@@ -76,7 +79,7 @@ class FwQueryConfig:
extra_ecus: List[Tuple[capnp.lib.capnp._EnumModule, int, Optional[int]]] = field(default_factory=list)
# Function a brand can implement to provide better fuzzy matching. Takes in FW versions,
# returns set of candidates. Only will match if one candidate is returned
match_fw_to_car_fuzzy: Optional[Callable[[Dict[Tuple[int, Optional[int]], Set[bytes]]], Set[str]]] = None
match_fw_to_car_fuzzy: Optional[Callable[[Dict[AddrType, Set[bytes]]], Set[str]]] = None
def __post_init__(self):
for i in range(len(self.requests)):
selfdrive/car/fw_versions.py
+15 -10
View File
@@ -1,13 +1,14 @@
#!/usr/bin/env python3
from collections import defaultdict
from typing import Any, DefaultDict, Dict, List, Optional, Set, Tuple
from typing import Any, DefaultDict, Dict, List, Optional, Set
from tqdm import tqdm
import capnp
import panda.python.uds as uds
from cereal import car
from common.params import Params
from selfdrive.car.ecu_addrs import EcuAddrBusType, get_ecu_addrs
from selfdrive.car.ecu_addrs import get_ecu_addrs
from selfdrive.car.fw_query_definitions import AddrType, EcuAddrBusType
from selfdrive.car.interfaces import get_interface_attr
from selfdrive.car.fingerprints import FW_VERSIONS
from selfdrive.car.isotp_parallel_query import IsoTpParallelQuery
@@ -35,8 +36,8 @@ def is_brand(brand: str, filter_brand: Optional[str]) -> bool:
def build_fw_dict(fw_versions: List[capnp.lib.capnp._DynamicStructBuilder],
filter_brand: Optional[str] = None) -> Dict[Tuple[int, Optional[int]], Set[bytes]]:
fw_versions_dict = defaultdict(set)
filter_brand: Optional[str] = None) -> Dict[AddrType, Set[bytes]]:
fw_versions_dict: DefaultDict[AddrType, Set[bytes]] = defaultdict(set)
for fw in fw_versions:
if is_brand(fw.brand, filter_brand) and not fw.logging:
sub_addr = fw.subAddress if fw.subAddress != 0 else None
@@ -44,8 +45,8 @@ def build_fw_dict(fw_versions: List[capnp.lib.capnp._DynamicStructBuilder],
return dict(fw_versions_dict)
def get_brand_addrs() -> Dict[str, Set[Tuple[int, Optional[int]]]]:
brand_addrs: DefaultDict[str, Set[Tuple[int, Optional[int]]]] = defaultdict(set)
def get_brand_addrs() -> Dict[str, Set[AddrType]]:
brand_addrs: DefaultDict[str, Set[AddrType]] = defaultdict(set)
for brand, cars in VERSIONS.items():
# Add ecus in database + extra ecus to match against
brand_addrs[brand] |= {(addr, sub_addr) for _, addr, sub_addr in FW_QUERY_CONFIGS[brand].extra_ecus}
@@ -54,7 +55,7 @@ def get_brand_addrs() -> Dict[str, Set[Tuple[int, Optional[int]]]]:
return dict(brand_addrs)
def match_fw_to_car_fuzzy(live_fw_versions, log=True, exclude=None):
def match_fw_to_car_fuzzy(live_fw_versions, match_brand=None, log=True, exclude=None):
"""Do a fuzzy FW match. This function will return a match, and the number of firmware version
that were matched uniquely to that specific car. If multiple ECUs uniquely match to different cars
the match is rejected."""
@@ -62,6 +63,9 @@ def match_fw_to_car_fuzzy(live_fw_versions, log=True, exclude=None):
# Build lookup table from (addr, sub_addr, fw) to list of candidate cars
all_fw_versions = defaultdict(list)
for candidate, fw_by_addr in FW_VERSIONS.items():
if not is_brand(MODEL_TO_BRAND[candidate], match_brand):
continue
if candidate == exclude:
continue
@@ -101,13 +105,14 @@ def match_fw_to_car_fuzzy(live_fw_versions, log=True, exclude=None):
return set()
def match_fw_to_car_exact(live_fw_versions, log=True) -> Set[str]:
def match_fw_to_car_exact(live_fw_versions, match_brand=None, log=True) -> Set[str]:
"""Do an exact FW match. Returns all cars that match the given
FW versions for a list of "essential" ECUs. If an ECU is not considered
essential the FW version can be missing to get a fingerprint, but if it's present it
needs to match the database."""
invalid = set()
candidates = FW_VERSIONS
candidates = {c: f for c, f in FW_VERSIONS.items() if
is_brand(MODEL_TO_BRAND[c], match_brand)}
for candidate, fws in candidates.items():
config = FW_QUERY_CONFIGS[MODEL_TO_BRAND[candidate]]
@@ -149,7 +154,7 @@ def match_fw_to_car(fw_versions, allow_exact=True, allow_fuzzy=True, log=True):
matches = set()
for brand in VERSIONS.keys():
fw_versions_dict = build_fw_dict(fw_versions, filter_brand=brand)
matches |= match_func(fw_versions_dict, log=log)
matches |= match_func(fw_versions_dict, match_brand=brand, log=log)
# If specified and no matches so far, fall back to brand's fuzzy fingerprinting function
config = FW_QUERY_CONFIGS[brand]
selfdrive/car/gm/interface.py
+20 -25
View File
@@ -4,7 +4,7 @@ from math import fabs, exp
from panda import Panda
from common.conversions import Conversions as CV
from selfdrive.car import STD_CARGO_KG, create_button_event, scale_tire_stiffness, get_safety_config
from selfdrive.car import create_button_event, get_safety_config
from selfdrive.car.gm.radar_interface import RADAR_HEADER_MSG
from selfdrive.car.gm.values import CAR, CruiseButtons, CarControllerParams, EV_CAR, CAMERA_ACC_CAR, CanBus
from selfdrive.car.interfaces import CarInterfaceBase, TorqueFromLateralAccelCallbackType, FRICTION_THRESHOLD
@@ -128,17 +128,17 @@ class CarInterface(CarInterfaceBase):
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2], [0.00]]
ret.lateralTuning.pid.kf = 0.00004 # full torque for 20 deg at 80mph means 0.00007818594
ret.steerActuatorDelay = 0.1 # Default delay, not measured yet
tire_stiffness_factor = 0.444 # not optimized yet
ret.tireStiffnessFactor = 0.444 # not optimized yet
ret.steerLimitTimer = 0.4
ret.radarTimeStep = 0.0667 # GM radar runs at 15Hz instead of standard 20Hz
ret.longitudinalActuatorDelayUpperBound = 0.5 # large delay to initially start braking
if candidate == CAR.VOLT:
ret.mass = 1607. + STD_CARGO_KG
ret.mass = 1607.
ret.wheelbase = 2.69
ret.steerRatio = 17.7 # Stock 15.7, LiveParameters
tire_stiffness_factor = 0.469 # Stock Michelin Energy Saver A/S, LiveParameters
ret.tireStiffnessFactor = 0.469 # Stock Michelin Energy Saver A/S, LiveParameters
ret.centerToFront = ret.wheelbase * 0.45 # Volt Gen 1, TODO corner weigh
ret.lateralTuning.pid.kpBP = [0., 40.]
@@ -149,13 +149,13 @@ class CarInterface(CarInterfaceBase):
ret.steerActuatorDelay = 0.2
elif candidate == CAR.MALIBU:
ret.mass = 1496. + STD_CARGO_KG
ret.mass = 1496.
ret.wheelbase = 2.83
ret.steerRatio = 15.8
ret.centerToFront = ret.wheelbase * 0.4 # wild guess
elif candidate == CAR.HOLDEN_ASTRA:
ret.mass = 1363. + STD_CARGO_KG
ret.mass = 1363.
ret.wheelbase = 2.662
# Remaining parameters copied from Volt for now
ret.centerToFront = ret.wheelbase * 0.4
@@ -163,7 +163,7 @@ class CarInterface(CarInterfaceBase):
elif candidate == CAR.ACADIA:
ret.minEnableSpeed = -1. # engage speed is decided by pcm
ret.mass = 4353. * CV.LB_TO_KG + STD_CARGO_KG
ret.mass = 4353. * CV.LB_TO_KG
ret.wheelbase = 2.86
ret.steerRatio = 14.4 # end to end is 13.46
ret.centerToFront = ret.wheelbase * 0.4
@@ -171,27 +171,27 @@ class CarInterface(CarInterfaceBase):
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
elif candidate == CAR.BUICK_LACROSSE:
ret.mass = 1712. + STD_CARGO_KG
ret.mass = 1712.
ret.wheelbase = 2.91
ret.steerRatio = 15.8
ret.centerToFront = ret.wheelbase * 0.4 # wild guess
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
elif candidate == CAR.BUICK_REGAL:
ret.mass = 3779. * CV.LB_TO_KG + STD_CARGO_KG # (3849+3708)/2
ret.mass = 3779. * CV.LB_TO_KG # (3849+3708)/2
ret.wheelbase = 2.83 # 111.4 inches in meters
ret.steerRatio = 14.4 # guess for tourx
ret.centerToFront = ret.wheelbase * 0.4 # guess for tourx
elif candidate == CAR.CADILLAC_ATS:
ret.mass = 1601. + STD_CARGO_KG
ret.mass = 1601.
ret.wheelbase = 2.78
ret.steerRatio = 15.3
ret.centerToFront = ret.wheelbase * 0.5
elif candidate == CAR.ESCALADE:
ret.minEnableSpeed = -1. # engage speed is decided by pcm
ret.mass = 5653. * CV.LB_TO_KG + STD_CARGO_KG # (5552+5815)/2
ret.mass = 5653. * CV.LB_TO_KG # (5552+5815)/2
ret.wheelbase = 2.95 # 116 inches in meters
ret.steerRatio = 17.3
ret.centerToFront = ret.wheelbase * 0.5
@@ -199,30 +199,30 @@ class CarInterface(CarInterfaceBase):
elif candidate == CAR.ESCALADE_ESV:
ret.minEnableSpeed = -1. # engage speed is decided by pcm
ret.mass = 2739. + STD_CARGO_KG
ret.mass = 2739.
ret.wheelbase = 3.302
ret.steerRatio = 17.3
ret.centerToFront = ret.wheelbase * 0.5
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[10., 41.0], [10., 41.0]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.13, 0.24], [0.01, 0.02]]
ret.lateralTuning.pid.kf = 0.000045
tire_stiffness_factor = 1.0
ret.tireStiffnessFactor = 1.0
elif candidate == CAR.BOLT_EUV:
ret.mass = 1669. + STD_CARGO_KG
ret.mass = 1669.
ret.wheelbase = 2.63779
ret.steerRatio = 16.8
ret.centerToFront = ret.wheelbase * 0.4
tire_stiffness_factor = 1.0
ret.tireStiffnessFactor = 1.0
ret.steerActuatorDelay = 0.2
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
elif candidate == CAR.SILVERADO:
ret.mass = 2200. + STD_CARGO_KG
ret.mass = 2450.
ret.wheelbase = 3.75
ret.steerRatio = 16.3
ret.centerToFront = ret.wheelbase * 0.5
tire_stiffness_factor = 1.0
ret.tireStiffnessFactor = 1.0
# On the Bolt, the ECM and camera independently check that you are either above 5 kph or at a stop
# with foot on brake to allow engagement, but this platform only has that check in the camera.
# TODO: check if this is split by EV/ICE with more platforms in the future
@@ -231,26 +231,21 @@ class CarInterface(CarInterfaceBase):
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
elif candidate == CAR.EQUINOX:
ret.mass = 3500. * CV.LB_TO_KG + STD_CARGO_KG
ret.mass = 3500. * CV.LB_TO_KG
ret.wheelbase = 2.72
ret.steerRatio = 14.4
ret.centerToFront = ret.wheelbase * 0.4
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
elif candidate == CAR.TRAILBLAZER:
ret.mass = 1345. + STD_CARGO_KG
ret.mass = 1345.
ret.wheelbase = 2.64
ret.steerRatio = 16.8
ret.centerToFront = ret.wheelbase * 0.4
tire_stiffness_factor = 1.0
ret.tireStiffnessFactor = 1.0
ret.steerActuatorDelay = 0.2
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
# TODO: start from empirically derived lateral slip stiffness for the civic and scale by
# mass and CG position, so all cars will have approximately similar dyn behaviors
ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront,
tire_stiffness_factor=tire_stiffness_factor)
return ret
# returns a car.CarState
selfdrive/car/gm/radar_interface.py
+2 -2
View File
@@ -25,7 +25,7 @@ def create_radar_can_parser(car_fingerprint):
['TrkRange'] * NUM_SLOTS + ['TrkRangeRate'] * NUM_SLOTS +
['TrkRangeAccel'] * NUM_SLOTS + ['TrkAzimuth'] * NUM_SLOTS +
['TrkWidth'] * NUM_SLOTS + ['TrkObjectID'] * NUM_SLOTS,
[RADAR_HEADER_MSG] * 7 + radar_targets * 6))
[RADAR_HEADER_MSG] * 7 + radar_targets * 6, strict=True))
messages = list({(s[1], 14) for s in signals})
@@ -44,7 +44,7 @@ class RadarInterface(RadarInterfaceBase):
def update(self, can_strings):
if self.rcp is None:
return super().update(None)
return None
vls = self.rcp.update_strings(can_strings)
self.updated_messages.update(vls)
selfdrive/car/gm/values.py
+3 -3
View File
@@ -212,10 +212,10 @@ FINGERPRINTS = {
}],
# Trailblazer also matches as a Silverado, so comment out to avoid conflicts.
# TODO: split with FW versions
CAR.TRAILBLAZER: [
{
# CAR.TRAILBLAZER: [
# {
# 190: 6, 193: 8, 197: 8, 201: 8, 209: 7, 211: 2, 241: 6, 249: 8, 288: 5, 289: 8, 298: 8, 304: 3, 309: 8, 311: 8, 313: 8, 320: 4, 328: 1, 352: 5, 381: 8, 384: 4, 386: 8, 388: 8, 413: 8, 451: 8, 452: 8, 453: 6, 455: 7, 479: 3, 481: 7, 485: 8, 489: 8, 497: 8, 500: 6, 501: 8, 532: 6, 560: 8, 562: 8, 563: 5, 565: 5, 587: 8, 707: 8, 715: 8, 717: 5, 761: 7, 789: 5, 800: 6, 810: 8, 840: 5, 842: 5, 844: 8, 869: 4, 880: 6, 977: 8, 1001: 8, 1011: 6, 1017: 8, 1020: 8, 1217: 8, 1221: 5, 1233: 8, 1249: 8, 1259: 8, 1261: 7, 1263: 4, 1265: 8, 1267: 1, 1271: 8, 1280: 4, 1296: 4, 1300: 8, 1609: 8, 1611: 8, 1613: 8, 1649: 8, 1792: 8, 1798: 8, 1824: 8, 1825: 8, 1840: 8, 1842: 8, 1858: 8, 1860: 8, 1863: 8, 1872: 8, 1875: 8, 1882: 8, 1888: 8, 1889: 8, 1892: 8, 1930: 7, 1937: 8, 1953: 8, 1968: 8, 2001: 8, 2017: 8, 2018: 8, 2020: 8
}],
# }],
}
GM_RX_OFFSET = 0x400
selfdrive/car/honda/interface.py
+37 -44
View File
@@ -5,7 +5,7 @@ from common.conversions import Conversions as CV
from common.numpy_fast import interp
from selfdrive.car.honda.values import CarControllerParams, CruiseButtons, HondaFlags, CAR, HONDA_BOSCH, HONDA_NIDEC_ALT_SCM_MESSAGES, \
HONDA_BOSCH_ALT_BRAKE_SIGNAL, HONDA_BOSCH_RADARLESS
from selfdrive.car import STD_CARGO_KG, CivicParams, create_button_event, scale_tire_stiffness, get_safety_config
from selfdrive.car import create_button_event, get_safety_config
from selfdrive.car.interfaces import CarInterfaceBase
from selfdrive.car.disable_ecu import disable_ecu
@@ -89,9 +89,9 @@ class CarInterface(CarInterfaceBase):
eps_modified = True
if candidate == CAR.CIVIC:
ret.mass = CivicParams.MASS
ret.wheelbase = CivicParams.WHEELBASE
ret.centerToFront = CivicParams.CENTER_TO_FRONT
ret.mass = 1326.
ret.wheelbase = 2.70
ret.centerToFront = ret.wheelbase * 0.4
ret.steerRatio = 15.38 # 10.93 is end-to-end spec
if eps_modified:
# stock request input values: 0x0000, 0x00DE, 0x014D, 0x01EF, 0x0290, 0x0377, 0x0454, 0x0610, 0x06EE
@@ -105,24 +105,22 @@ class CarInterface(CarInterfaceBase):
else:
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 2560], [0, 2560]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[1.1], [0.33]]
tire_stiffness_factor = 1.
elif candidate in (CAR.CIVIC_BOSCH, CAR.CIVIC_BOSCH_DIESEL, CAR.CIVIC_2022):
ret.mass = CivicParams.MASS
ret.wheelbase = CivicParams.WHEELBASE
ret.centerToFront = CivicParams.CENTER_TO_FRONT
ret.mass = 1326.
ret.wheelbase = 2.70
ret.centerToFront = ret.wheelbase * 0.4
ret.steerRatio = 15.38 # 10.93 is end-to-end spec
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end
tire_stiffness_factor = 1.
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.8], [0.24]]
elif candidate in (CAR.ACCORD, CAR.ACCORDH):
ret.mass = 3279. * CV.LB_TO_KG + STD_CARGO_KG
ret.mass = 3279. * CV.LB_TO_KG
ret.wheelbase = 2.83
ret.centerToFront = ret.wheelbase * 0.39
ret.steerRatio = 16.33 # 11.82 is spec end-to-end
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end
tire_stiffness_factor = 0.8467
ret.tireStiffnessFactor = 0.8467
if eps_modified:
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.3], [0.09]]
@@ -130,26 +128,26 @@ class CarInterface(CarInterfaceBase):
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.18]]
elif candidate == CAR.ACURA_ILX:
ret.mass = 3095. * CV.LB_TO_KG + STD_CARGO_KG
ret.mass = 3095. * CV.LB_TO_KG
ret.wheelbase = 2.67
ret.centerToFront = ret.wheelbase * 0.37
ret.steerRatio = 18.61 # 15.3 is spec end-to-end
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 3840], [0, 3840]] # TODO: determine if there is a dead zone at the top end
tire_stiffness_factor = 0.72
ret.tireStiffnessFactor = 0.72
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.8], [0.24]]
elif candidate in (CAR.CRV, CAR.CRV_EU):
ret.mass = 3572. * CV.LB_TO_KG + STD_CARGO_KG
ret.mass = 3572. * CV.LB_TO_KG
ret.wheelbase = 2.62
ret.centerToFront = ret.wheelbase * 0.41
ret.steerRatio = 16.89 # as spec
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 1000], [0, 1000]] # TODO: determine if there is a dead zone at the top end
tire_stiffness_factor = 0.444
ret.tireStiffnessFactor = 0.444
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.8], [0.24]]
ret.wheelSpeedFactor = 1.025
elif candidate == CAR.CRV_5G:
ret.mass = 3410. * CV.LB_TO_KG + STD_CARGO_KG
ret.mass = 3410. * CV.LB_TO_KG
ret.wheelbase = 2.66
ret.centerToFront = ret.wheelbase * 0.41
ret.steerRatio = 16.0 # 12.3 is spec end-to-end
@@ -162,45 +160,45 @@ class CarInterface(CarInterfaceBase):
else:
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 3840], [0, 3840]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.64], [0.192]]
tire_stiffness_factor = 0.677
ret.tireStiffnessFactor = 0.677
ret.wheelSpeedFactor = 1.025
elif candidate == CAR.CRV_HYBRID:
ret.mass = 1667. + STD_CARGO_KG # mean of 4 models in kg
ret.mass = 1667. # mean of 4 models in kg
ret.wheelbase = 2.66
ret.centerToFront = ret.wheelbase * 0.41
ret.steerRatio = 16.0 # 12.3 is spec end-to-end
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end
tire_stiffness_factor = 0.677
ret.tireStiffnessFactor = 0.677
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.18]]
ret.wheelSpeedFactor = 1.025
elif candidate == CAR.FIT:
ret.mass = 2644. * CV.LB_TO_KG + STD_CARGO_KG
ret.mass = 2644. * CV.LB_TO_KG
ret.wheelbase = 2.53
ret.centerToFront = ret.wheelbase * 0.39
ret.steerRatio = 13.06
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end
tire_stiffness_factor = 0.75
ret.tireStiffnessFactor = 0.75
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2], [0.05]]
elif candidate == CAR.FREED:
ret.mass = 3086. * CV.LB_TO_KG + STD_CARGO_KG
ret.mass = 3086. * CV.LB_TO_KG
ret.wheelbase = 2.74
# the remaining parameters were copied from FIT
ret.centerToFront = ret.wheelbase * 0.39
ret.steerRatio = 13.06
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]]
tire_stiffness_factor = 0.75
ret.tireStiffnessFactor = 0.75
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2], [0.05]]
elif candidate in (CAR.HRV, CAR.HRV_3G):
ret.mass = 3125 * CV.LB_TO_KG + STD_CARGO_KG
ret.mass = 3125 * CV.LB_TO_KG
ret.wheelbase = 2.61
ret.centerToFront = ret.wheelbase * 0.41
ret.steerRatio = 15.2
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]]
tire_stiffness_factor = 0.5
ret.tireStiffnessFactor = 0.5
if candidate == CAR.HRV:
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.16], [0.025]]
ret.wheelSpeedFactor = 1.025
@@ -208,29 +206,29 @@ class CarInterface(CarInterfaceBase):
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.8], [0.24]] # TODO: can probably use some tuning
elif candidate == CAR.ACURA_RDX:
ret.mass = 3935. * CV.LB_TO_KG + STD_CARGO_KG
ret.mass = 3935. * CV.LB_TO_KG
ret.wheelbase = 2.68
ret.centerToFront = ret.wheelbase * 0.38
ret.steerRatio = 15.0 # as spec
tire_stiffness_factor = 0.444
ret.tireStiffnessFactor = 0.444
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 1000], [0, 1000]] # TODO: determine if there is a dead zone at the top end
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.8], [0.24]]
elif candidate == CAR.ACURA_RDX_3G:
ret.mass = 4068. * CV.LB_TO_KG + STD_CARGO_KG
ret.mass = 4068. * CV.LB_TO_KG
ret.wheelbase = 2.75
ret.centerToFront = ret.wheelbase * 0.41
ret.steerRatio = 11.95 # as spec
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 3840], [0, 3840]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2], [0.06]]
tire_stiffness_factor = 0.677
ret.tireStiffnessFactor = 0.677
elif candidate in (CAR.ODYSSEY, CAR.ODYSSEY_CHN):
ret.mass = 1900. + STD_CARGO_KG
ret.mass = 1900.
ret.wheelbase = 3.00
ret.centerToFront = ret.wheelbase * 0.41
ret.steerRatio = 14.35 # as spec
tire_stiffness_factor = 0.82
ret.tireStiffnessFactor = 0.82
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.28], [0.08]]
if candidate == CAR.ODYSSEY_CHN:
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 32767], [0, 32767]] # TODO: determine if there is a dead zone at the top end
@@ -238,39 +236,39 @@ class CarInterface(CarInterfaceBase):
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end
elif candidate == CAR.PILOT:
ret.mass = 4278. * CV.LB_TO_KG + STD_CARGO_KG # average weight
ret.mass = 4278. * CV.LB_TO_KG # average weight
ret.wheelbase = 2.86
ret.centerToFront = ret.wheelbase * 0.428
ret.steerRatio = 16.0 # as spec
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end
tire_stiffness_factor = 0.444
ret.tireStiffnessFactor = 0.444
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.38], [0.11]]
elif candidate == CAR.RIDGELINE:
ret.mass = 4515. * CV.LB_TO_KG + STD_CARGO_KG
ret.mass = 4515. * CV.LB_TO_KG
ret.wheelbase = 3.18
ret.centerToFront = ret.wheelbase * 0.41
ret.steerRatio = 15.59 # as spec
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end
tire_stiffness_factor = 0.444
ret.tireStiffnessFactor = 0.444
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.38], [0.11]]
elif candidate == CAR.INSIGHT:
ret.mass = 2987. * CV.LB_TO_KG + STD_CARGO_KG
ret.mass = 2987. * CV.LB_TO_KG
ret.wheelbase = 2.7
ret.centerToFront = ret.wheelbase * 0.39
ret.steerRatio = 15.0 # 12.58 is spec end-to-end
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end
tire_stiffness_factor = 0.82
ret.tireStiffnessFactor = 0.82
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.18]]
elif candidate == CAR.HONDA_E:
ret.mass = 3338.8 * CV.LB_TO_KG + STD_CARGO_KG
ret.mass = 3338.8 * CV.LB_TO_KG
ret.wheelbase = 2.5
ret.centerToFront = ret.wheelbase * 0.5
ret.steerRatio = 16.71
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end
tire_stiffness_factor = 0.82
ret.tireStiffnessFactor = 0.82
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.18]] # TODO: can probably use some tuning
else:
@@ -296,11 +294,6 @@ class CarInterface(CarInterfaceBase):
ret.autoResumeSng = candidate in (HONDA_BOSCH | {CAR.CIVIC}) or ret.enableGasInterceptor
ret.minEnableSpeed = -1. if ret.autoResumeSng else 25.5 * CV.MPH_TO_MS
# TODO: start from empirically derived lateral slip stiffness for the civic and scale by
# mass and CG position, so all cars will have approximately similar dyn behaviors
ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront,
tire_stiffness_factor=tire_stiffness_factor)
ret.steerActuatorDelay = 0.1
ret.steerLimitTimer = 0.8
selfdrive/car/honda/radar_interface.py
+45 -32
View File
@@ -1,4 +1,6 @@
#!/usr/bin/env python3
from collections import defaultdict
from cereal import car
from opendbc.can.parser import CANParser
from selfdrive.car.interfaces import RadarInterfaceBase
@@ -28,50 +30,61 @@ class RadarInterface(RadarInterfaceBase):
else:
self.rcp = _create_nidec_can_parser(CP.carFingerprint)
self.trigger_msg = 0x445
self.updated_messages = set()
self.updated_values = defaultdict(lambda: defaultdict(list))
def update(self, can_strings):
# in Bosch radar and we are only steering for now, so sleep 0.05s to keep
# radard at 20Hz and return no points
if self.radar_off_can:
return super().update(None)
vls = self.rcp.update_strings(can_strings)
self.updated_messages.update(vls)
if self.trigger_msg not in self.updated_messages:
return None
rr = self._update(self.updated_messages)
self.updated_messages.clear()
return rr
addresses = self.rcp.update_strings(can_strings)
for addr in addresses:
vals_dict = self.rcp.vl_all[addr]
for sig_name, vals in vals_dict.items():
self.updated_values[addr][sig_name].extend(vals)
def _update(self, updated_messages):
if self.trigger_msg not in self.updated_values:
return None
radar_data = self._radar_msg_from_buffer(self.updated_values, self.rcp.can_valid)
self.updated_values.clear()
return radar_data
def _radar_msg_from_buffer(self, updated_values, can_valid):
ret = car.RadarData.new_message()
for ii in sorted(updated_messages):
cpt = self.rcp.vl[ii]
if ii == 0x400:
# check for radar faults
self.radar_fault = cpt['RADAR_STATE'] != 0x79
self.radar_wrong_config = cpt['RADAR_STATE'] == 0x69
elif cpt['LONG_DIST'] < 255:
if ii not in self.pts or cpt['NEW_TRACK']:
self.pts[ii] = car.RadarData.RadarPoint.new_message()
self.pts[ii].trackId = self.track_id
self.track_id += 1
self.pts[ii].dRel = cpt['LONG_DIST'] # from front of car
self.pts[ii].yRel = -cpt['LAT_DIST'] # in car frame's y axis, left is positive
self.pts[ii].vRel = cpt['REL_SPEED']
self.pts[ii].aRel = float('nan')
self.pts[ii].yvRel = float('nan')
self.pts[ii].measured = True
else:
if ii in self.pts:
del self.pts[ii]
for ii in sorted(updated_values):
msgs = updated_values[ii]
n_vals_per_addr = len(list(msgs.values())[0])
cpts = [
{k: v[i] for k, v in msgs.items()}
for i in range(n_vals_per_addr)
]
for cpt in cpts:
if ii == 0x400:
# check for radar faults
self.radar_fault = cpt['RADAR_STATE'] != 0x79
self.radar_wrong_config = cpt['RADAR_STATE'] == 0x69
elif cpt['LONG_DIST'] < 255:
if ii not in self.pts or cpt['NEW_TRACK']:
self.pts[ii] = car.RadarData.RadarPoint.new_message()
self.pts[ii].trackId = self.track_id
self.track_id += 1
self.pts[ii].dRel = cpt['LONG_DIST'] # from front of car
self.pts[ii].yRel = -cpt['LAT_DIST'] # in car frame's y axis, left is positive
self.pts[ii].vRel = cpt['REL_SPEED']
self.pts[ii].aRel = float('nan')
self.pts[ii].yvRel = float('nan')
self.pts[ii].measured = True
else:
if ii in self.pts:
del self.pts[ii]
errors = []
if not self.rcp.can_valid:
if not can_valid:
errors.append("canError")
if self.radar_fault:
errors.append("fault")
selfdrive/car/hyundai/carstate.py
+22 -38
View File
@@ -12,6 +12,7 @@ from selfdrive.car.interfaces import CarStateBase
PREV_BUTTON_SAMPLES = 8
CLUSTER_SAMPLE_RATE = 20 # frames
STANDSTILL_THRESHOLD = 12 * 0.03125 * CV.KPH_TO_MS
class CarState(CarStateBase):
@@ -22,8 +23,8 @@ class CarState(CarStateBase):
self.cruise_buttons = deque([Buttons.NONE] * PREV_BUTTON_SAMPLES, maxlen=PREV_BUTTON_SAMPLES)
self.main_buttons = deque([Buttons.NONE] * PREV_BUTTON_SAMPLES, maxlen=PREV_BUTTON_SAMPLES)
self.gear_msg_canfd = "GEAR_ALT_2" if CP.flags & HyundaiFlags.CANFD_ALT_GEARS_2 else \
"GEAR_ALT" if CP.flags & HyundaiFlags.CANFD_ALT_GEARS else \
self.gear_msg_canfd = "GEAR_ALT" if CP.flags & HyundaiFlags.CANFD_ALT_GEARS else \
"GEAR_ALT_2" if CP.flags & HyundaiFlags.CANFD_ALT_GEARS_2 else \
"GEAR_SHIFTER"
if CP.carFingerprint in CANFD_CAR:
self.shifter_values = can_define.dv[self.gear_msg_canfd]["GEAR"]
@@ -34,6 +35,11 @@ class CarState(CarStateBase):
else: # preferred and elect gear methods use same definition
self.shifter_values = can_define.dv["LVR12"]["CF_Lvr_Gear"]
self.accelerator_msg_canfd = "ACCELERATOR" if CP.carFingerprint in EV_CAR else \
"ACCELERATOR_ALT" if CP.carFingerprint in HYBRID_CAR else \
"ACCELERATOR_BRAKE_ALT"
self.cruise_btns_msg_canfd = "CRUISE_BUTTONS_ALT" if CP.flags & HyundaiFlags.CANFD_ALT_BUTTONS else \
"CRUISE_BUTTONS"
self.is_metric = False
self.buttons_counter = 0
@@ -67,7 +73,7 @@ class CarState(CarStateBase):
)
ret.vEgoRaw = (ret.wheelSpeeds.fl + ret.wheelSpeeds.fr + ret.wheelSpeeds.rl + ret.wheelSpeeds.rr) / 4.
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
ret.standstill = ret.vEgoRaw < 0.1
ret.standstill = ret.wheelSpeeds.fl <= STANDSTILL_THRESHOLD and ret.wheelSpeeds.rr <= STANDSTILL_THRESHOLD
self.cluster_speed_counter += 1
if self.cluster_speed_counter > CLUSTER_SAMPLE_RATE:
@@ -166,13 +172,11 @@ class CarState(CarStateBase):
speed_factor = CV.KPH_TO_MS if self.is_metric else CV.MPH_TO_MS
if self.CP.carFingerprint in (EV_CAR | HYBRID_CAR):
if self.CP.carFingerprint in EV_CAR:
ret.gas = cp.vl["ACCELERATOR"]["ACCELERATOR_PEDAL"] / 255.
else:
ret.gas = cp.vl["ACCELERATOR_ALT"]["ACCELERATOR_PEDAL"] / 1023.
offset = 255. if self.CP.carFingerprint in EV_CAR else 1023.
ret.gas = cp.vl[self.accelerator_msg_canfd]["ACCELERATOR_PEDAL"] / offset
ret.gasPressed = ret.gas > 1e-5
else:
ret.gasPressed = bool(cp.vl["ACCELERATOR_BRAKE_ALT"]["ACCELERATOR_PEDAL_PRESSED"])
ret.gasPressed = bool(cp.vl[self.accelerator_msg_canfd]["ACCELERATOR_PEDAL_PRESSED"])
ret.brakePressed = cp.vl["TCS"]["DriverBraking"] == 1
@@ -191,7 +195,7 @@ class CarState(CarStateBase):
)
ret.vEgoRaw = (ret.wheelSpeeds.fl + ret.wheelSpeeds.fr + ret.wheelSpeeds.rl + ret.wheelSpeeds.rr) / 4.
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
ret.standstill = ret.vEgoRaw < 0.1
ret.standstill = ret.wheelSpeeds.fl <= STANDSTILL_THRESHOLD and ret.wheelSpeeds.rr <= STANDSTILL_THRESHOLD
ret.steeringRateDeg = cp.vl["STEERING_SENSORS"]["STEERING_RATE"]
ret.steeringAngleDeg = cp.vl["STEERING_SENSORS"]["STEERING_ANGLE"] * -1
@@ -220,11 +224,10 @@ class CarState(CarStateBase):
ret.cruiseState.speed = cp_cruise_info.vl["SCC_CONTROL"]["VSetDis"] * speed_factor
self.cruise_info = copy.copy(cp_cruise_info.vl["SCC_CONTROL"])
cruise_btn_msg = "CRUISE_BUTTONS_ALT" if self.CP.flags & HyundaiFlags.CANFD_ALT_BUTTONS else "CRUISE_BUTTONS"
self.prev_cruise_buttons = self.cruise_buttons[-1]
self.cruise_buttons.extend(cp.vl_all[cruise_btn_msg]["CRUISE_BUTTONS"])
self.main_buttons.extend(cp.vl_all[cruise_btn_msg]["ADAPTIVE_CRUISE_MAIN_BTN"])
self.buttons_counter = cp.vl[cruise_btn_msg]["COUNTER"]
self.cruise_buttons.extend(cp.vl_all[self.cruise_btns_msg_canfd]["CRUISE_BUTTONS"])
self.main_buttons.extend(cp.vl_all[self.cruise_btns_msg_canfd]["ADAPTIVE_CRUISE_MAIN_BTN"])
self.buttons_counter = cp.vl[self.cruise_btns_msg_canfd]["COUNTER"]
ret.accFaulted = cp.vl["TCS"]["ACCEnable"] != 0 # 0 ACC CONTROL ENABLED, 1-3 ACC CONTROL DISABLED
if self.CP.flags & HyundaiFlags.CANFD_HDA2:
@@ -232,10 +235,9 @@ class CarState(CarStateBase):
return ret
@staticmethod
def get_can_parser(CP):
def get_can_parser(self, CP):
if CP.carFingerprint in CANFD_CAR:
return CarState.get_can_parser_canfd(CP)
return self.get_can_parser_canfd(CP)
freq_mdps12 = 100 if CP.flags & HyundaiFlags.CAN_CANFD_HDA2.value else 50
@@ -315,14 +317,12 @@ class CarState(CarStateBase):
bus = 6 if CP.flags & HyundaiFlags.CAN_CANFD_HDA2 else 2
return CANParser(DBC[CP.carFingerprint]["pt"], messages, bus)
@staticmethod
def get_can_parser_canfd(CP):
gear_msg = "GEAR_ALT_2" if CP.flags & HyundaiFlags.CANFD_ALT_GEARS_2 else \
"GEAR_ALT" if CP.flags & HyundaiFlags.CANFD_ALT_GEARS else \
"GEAR_SHIFTER"
def get_can_parser_canfd(self, CP):
messages = [
(self.gear_msg_canfd, 100),
(self.cruise_btns_msg_canfd, 50),
(self.accelerator_msg_canfd, 100),
("WHEEL_SPEEDS", 100),
(gear_msg, 100),
("STEERING_SENSORS", 100),
("MDPS", 100),
("TCS", 50),
@@ -331,9 +331,6 @@ class CarState(CarStateBase):
("DOORS_SEATBELTS", 4),
]
if not (CP.flags & HyundaiFlags.CANFD_ALT_BUTTONS):
messages.append(("CRUISE_BUTTONS", 50))
if CP.enableBsm:
messages += [
("BLINDSPOTS_REAR_CORNERS", 20),
@@ -344,19 +341,6 @@ class CarState(CarStateBase):
("SCC_CONTROL", 50),
]
if CP.carFingerprint in EV_CAR:
messages += [
("ACCELERATOR", 100),
]
elif CP.carFingerprint in HYBRID_CAR:
messages += [
("ACCELERATOR_ALT", 100),
]
else:
messages += [
("ACCELERATOR_BRAKE_ALT", 100),
]
return CANParser(DBC[CP.carFingerprint]["pt"], messages, CanBus(CP).ECAN)
@staticmethod
selfdrive/car/hyundai/interface.py
+53 -59
View File
@@ -6,7 +6,7 @@ from selfdrive.car.hyundai.hyundaicanfd import CanBus
from selfdrive.car.hyundai.values import HyundaiFlags, CAR, DBC, CANFD_CAR, CAMERA_SCC_CAR, CANFD_RADAR_SCC_CAR, \
EV_CAR, HYBRID_CAR, LEGACY_SAFETY_MODE_CAR, Buttons
from selfdrive.car.hyundai.radar_interface import RADAR_START_ADDR
from selfdrive.car import STD_CARGO_KG, create_button_event, scale_tire_stiffness, get_safety_config
from selfdrive.car import create_button_event, get_safety_config
from selfdrive.car.interfaces import CarInterfaceBase
from selfdrive.car.disable_ecu import disable_ecu
@@ -63,185 +63,183 @@ class CarInterface(CarInterfaceBase):
ret.steerActuatorDelay = 0.1 # Default delay
ret.steerLimitTimer = 0.4
tire_stiffness_factor = 1.
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
if candidate in (CAR.SANTA_FE, CAR.SANTA_FE_2022, CAR.SANTA_FE_HEV_2022, CAR.SANTA_FE_PHEV_2022):
ret.mass = 3982. * CV.LB_TO_KG + STD_CARGO_KG
ret.mass = 3982. * CV.LB_TO_KG
ret.wheelbase = 2.766
# Values from optimizer
ret.steerRatio = 16.55 # 13.8 is spec end-to-end
tire_stiffness_factor = 0.82
ret.tireStiffnessFactor = 0.82
elif candidate in (CAR.SONATA, CAR.SONATA_HYBRID):
ret.mass = 1513. + STD_CARGO_KG
ret.mass = 1513.
ret.wheelbase = 2.84
ret.steerRatio = 13.27 * 1.15 # 15% higher at the center seems reasonable
tire_stiffness_factor = 0.65
ret.tireStiffnessFactor = 0.65
elif candidate == CAR.SONATA_LF:
ret.mass = 4497. * CV.LB_TO_KG
ret.mass = 1536.
ret.wheelbase = 2.804
ret.steerRatio = 13.27 * 1.15 # 15% higher at the center seems reasonable
elif candidate in (CAR.PALISADE, CAR.PALISADE_2023):
ret.mass = 1999. + STD_CARGO_KG
ret.mass = 1999.
ret.wheelbase = 2.90
ret.steerRatio = 15.6 * 1.15
tire_stiffness_factor = 0.63
ret.tireStiffnessFactor = 0.63
elif candidate == CAR.ELANTRA:
ret.mass = 1275. + STD_CARGO_KG
ret.mass = 1275.
ret.wheelbase = 2.7
ret.steerRatio = 15.4 # 14 is Stock | Settled Params Learner values are steerRatio: 15.401566348670535
tire_stiffness_factor = 0.385 # stiffnessFactor settled on 1.0081302973865127
ret.tireStiffnessFactor = 0.385 # stiffnessFactor settled on 1.0081302973865127
ret.minSteerSpeed = 32 * CV.MPH_TO_MS
elif candidate == CAR.ELANTRA_2021:
ret.mass = (2800. * CV.LB_TO_KG) + STD_CARGO_KG
ret.mass = 2800. * CV.LB_TO_KG
ret.wheelbase = 2.72
ret.steerRatio = 12.9
tire_stiffness_factor = 0.65
ret.tireStiffnessFactor = 0.65
elif candidate == CAR.ELANTRA_HEV_2021:
ret.mass = (3017. * CV.LB_TO_KG) + STD_CARGO_KG
ret.mass = 3017. * CV.LB_TO_KG
ret.wheelbase = 2.72
ret.steerRatio = 12.9
tire_stiffness_factor = 0.65
ret.tireStiffnessFactor = 0.65
elif candidate == CAR.HYUNDAI_GENESIS:
ret.mass = 2060. + STD_CARGO_KG
ret.mass = 2060.
ret.wheelbase = 3.01
ret.steerRatio = 16.5
ret.minSteerSpeed = 60 * CV.KPH_TO_MS
elif candidate in (CAR.KONA, CAR.KONA_EV, CAR.KONA_HEV, CAR.KONA_EV_2022):
ret.mass = {CAR.KONA_EV: 1685., CAR.KONA_HEV: 1425., CAR.KONA_EV_2022: 1743.}.get(candidate, 1275.) + STD_CARGO_KG
ret.mass = {CAR.KONA_EV: 1685., CAR.KONA_HEV: 1425., CAR.KONA_EV_2022: 1743.}.get(candidate, 1275.)
ret.wheelbase = 2.6
ret.steerRatio = 13.42 # Spec
tire_stiffness_factor = 0.385
ret.tireStiffnessFactor = 0.385
elif candidate in (CAR.IONIQ, CAR.IONIQ_EV_LTD, CAR.IONIQ_PHEV_2019, CAR.IONIQ_HEV_2022, CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV):
ret.mass = 1490. + STD_CARGO_KG # weight per hyundai site https://www.hyundaiusa.com/ioniq-electric/specifications.aspx
ret.mass = 1490. # weight per hyundai site https://www.hyundaiusa.com/ioniq-electric/specifications.aspx
ret.wheelbase = 2.7
ret.steerRatio = 13.73 # Spec
tire_stiffness_factor = 0.385
ret.tireStiffnessFactor = 0.385
if candidate in (CAR.IONIQ, CAR.IONIQ_EV_LTD, CAR.IONIQ_PHEV_2019):
ret.minSteerSpeed = 32 * CV.MPH_TO_MS
elif candidate == CAR.VELOSTER:
ret.mass = 3558. * CV.LB_TO_KG
ret.mass = 2917. * CV.LB_TO_KG
ret.wheelbase = 2.80
ret.steerRatio = 13.75 * 1.15
tire_stiffness_factor = 0.5
ret.tireStiffnessFactor = 0.5
elif candidate == CAR.TUCSON:
ret.mass = 3520. * CV.LB_TO_KG
ret.wheelbase = 2.67
ret.steerRatio = 14.00 * 1.15
tire_stiffness_factor = 0.385
ret.tireStiffnessFactor = 0.385
elif candidate in (CAR.TUCSON_4TH_GEN, CAR.TUCSON_HYBRID_4TH_GEN):
ret.mass = 1630. + STD_CARGO_KG # average
ret.mass = 1630. # average
ret.wheelbase = 2.756
ret.steerRatio = 16.
tire_stiffness_factor = 0.385
ret.tireStiffnessFactor = 0.385
elif candidate == CAR.SANTA_CRUZ_1ST_GEN:
ret.mass = 1870. + STD_CARGO_KG # weight from Limited trim - the only supported trim
ret.mass = 1870. # weight from Limited trim - the only supported trim
ret.wheelbase = 3.000
# steering ratio according to Hyundai News https://www.hyundainews.com/assets/documents/original/48035-2022SantaCruzProductGuideSpecsv2081521.pdf
ret.steerRatio = 14.2
# Kia
elif candidate == CAR.KIA_SORENTO:
ret.mass = 1985. + STD_CARGO_KG
ret.mass = 1985.
ret.wheelbase = 2.78
ret.steerRatio = 14.4 * 1.1 # 10% higher at the center seems reasonable
elif candidate in (CAR.KIA_NIRO_EV, CAR.KIA_NIRO_EV_2ND_GEN, CAR.KIA_NIRO_PHEV, CAR.KIA_NIRO_HEV_2021, CAR.KIA_NIRO_HEV_2ND_GEN):
ret.mass = 3543. * CV.LB_TO_KG + STD_CARGO_KG # average of all the cars
ret.mass = 3543. * CV.LB_TO_KG # average of all the cars
ret.wheelbase = 2.7
ret.steerRatio = 13.6 # average of all the cars
tire_stiffness_factor = 0.385
ret.tireStiffnessFactor = 0.385
if candidate == CAR.KIA_NIRO_PHEV:
ret.minSteerSpeed = 32 * CV.MPH_TO_MS
elif candidate == CAR.KIA_SELTOS:
ret.mass = 1337. + STD_CARGO_KG
ret.mass = 1337.
ret.wheelbase = 2.63
ret.steerRatio = 14.56
tire_stiffness_factor = 1
elif candidate == CAR.KIA_SPORTAGE_5TH_GEN:
ret.mass = 1700. + STD_CARGO_KG # weight from SX and above trims, average of FWD and AWD versions
ret.mass = 1700. # weight from SX and above trims, average of FWD and AWD versions
ret.wheelbase = 2.756
ret.steerRatio = 13.6 # steering ratio according to Kia News https://www.kiamedia.com/us/en/models/sportage/2023/specifications
elif candidate in (CAR.KIA_OPTIMA_G4, CAR.KIA_OPTIMA_G4_FL, CAR.KIA_OPTIMA_H):
ret.mass = 3558. * CV.LB_TO_KG
ret.wheelbase = 2.80
ret.steerRatio = 13.75
tire_stiffness_factor = 0.5
ret.tireStiffnessFactor = 0.5
if candidate == CAR.KIA_OPTIMA_G4:
ret.minSteerSpeed = 32 * CV.MPH_TO_MS
elif candidate in (CAR.KIA_STINGER, CAR.KIA_STINGER_2022):
ret.mass = 1825. + STD_CARGO_KG
ret.mass = 1825.
ret.wheelbase = 2.78
ret.steerRatio = 14.4 * 1.15 # 15% higher at the center seems reasonable
elif candidate == CAR.KIA_FORTE:
ret.mass = 3558. * CV.LB_TO_KG
ret.mass = 2878. * CV.LB_TO_KG
ret.wheelbase = 2.80
ret.steerRatio = 13.75
tire_stiffness_factor = 0.5
ret.tireStiffnessFactor = 0.5
elif candidate == CAR.KIA_CEED:
ret.mass = 1450. + STD_CARGO_KG
ret.mass = 1450.
ret.wheelbase = 2.65
ret.steerRatio = 13.75
tire_stiffness_factor = 0.5
ret.tireStiffnessFactor = 0.5
elif candidate in (CAR.KIA_K5_2021, CAR.KIA_K5_HEV_2020):
ret.mass = 3228. * CV.LB_TO_KG
ret.mass = 3381. * CV.LB_TO_KG
ret.wheelbase = 2.85
ret.steerRatio = 13.27 # 2021 Kia K5 Steering Ratio (all trims)
tire_stiffness_factor = 0.5
ret.tireStiffnessFactor = 0.5
elif candidate == CAR.KIA_EV6:
ret.mass = 2055 + STD_CARGO_KG
ret.mass = 2055
ret.wheelbase = 2.9
ret.steerRatio = 16.
tire_stiffness_factor = 0.65
ret.tireStiffnessFactor = 0.65
elif candidate in (CAR.IONIQ_5, CAR.IONIQ_6):
ret.mass = 1948 + STD_CARGO_KG
ret.mass = 1948
ret.wheelbase = 2.97
ret.steerRatio = 14.26
tire_stiffness_factor = 0.65
ret.tireStiffnessFactor = 0.65
elif candidate == CAR.KIA_SPORTAGE_HYBRID_5TH_GEN:
ret.mass = 1767. + STD_CARGO_KG # SX Prestige trim support only
ret.mass = 1767. # SX Prestige trim support only
ret.wheelbase = 2.756
ret.steerRatio = 13.6
elif candidate in (CAR.KIA_SORENTO_4TH_GEN, CAR.KIA_SORENTO_PHEV_4TH_GEN):
ret.wheelbase = 2.81
ret.steerRatio = 13.5 # average of the platforms
if candidate == CAR.KIA_SORENTO_4TH_GEN:
ret.mass = 3957 * CV.LB_TO_KG + STD_CARGO_KG
ret.mass = 3957 * CV.LB_TO_KG
else:
ret.mass = 4537 * CV.LB_TO_KG + STD_CARGO_KG
ret.mass = 4537 * CV.LB_TO_KG
elif candidate == CAR.KIA_CARNIVAL_4TH_GEN:
ret.mass = 2087. + STD_CARGO_KG
ret.mass = 2087.
ret.wheelbase = 3.09
ret.steerRatio = 14.23
# Genesis
elif candidate == CAR.GENESIS_GV60_EV_1ST_GEN:
ret.mass = 2205 + STD_CARGO_KG
ret.mass = 2205
ret.wheelbase = 2.9
# https://www.motor1.com/reviews/586376/2023-genesis-gv60-first-drive/#:~:text=Relative%20to%20the%20related%20Ioniq,5%2FEV6%27s%2014.3%3A1.
ret.steerRatio = 12.6
elif candidate == CAR.GENESIS_G70:
ret.steerActuatorDelay = 0.1
ret.mass = 1640.0 + STD_CARGO_KG
ret.mass = 1640.0
ret.wheelbase = 2.84
ret.steerRatio = 13.56
elif candidate == CAR.GENESIS_G70_2020:
ret.mass = 3673.0 * CV.LB_TO_KG + STD_CARGO_KG
ret.mass = 3673.0 * CV.LB_TO_KG
ret.wheelbase = 2.83
ret.steerRatio = 12.9
elif candidate == CAR.GENESIS_GV70_1ST_GEN:
ret.mass = 1950. + STD_CARGO_KG
ret.mass = 1950.
ret.wheelbase = 2.87
ret.steerRatio = 14.6
elif candidate == CAR.GENESIS_G80:
ret.mass = 2060. + STD_CARGO_KG
ret.mass = 2060.
ret.wheelbase = 3.01
ret.steerRatio = 16.5
elif candidate == CAR.GENESIS_G90:
ret.mass = 2200
ret.mass = 2200.
ret.wheelbase = 3.15
ret.steerRatio = 12.069
elif candidate == CAR.GENESIS_GV80:
ret.mass = 2258. + STD_CARGO_KG
ret.mass = 2258.
ret.wheelbase = 2.95
ret.steerRatio = 14.14
@@ -313,10 +311,6 @@ class CarInterface(CarInterfaceBase):
ret.centerToFront = ret.wheelbase * 0.4
# TODO: start from empirically derived lateral slip stiffness for the civic and scale by
# mass and CG position, so all cars will have approximately similar dyn behaviors
ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront,
tire_stiffness_factor=tire_stiffness_factor)
return ret
@staticmethod
selfdrive/car/hyundai/radar_interface.py
+1 -1
View File
@@ -30,7 +30,7 @@ class RadarInterface(RadarInterfaceBase):
def update(self, can_strings):
if self.radar_off_can or (self.rcp is None):
return super().update(None)
return None
vls = self.rcp.update_strings(can_strings)
self.updated_messages.update(vls)
selfdrive/car/interfaces.py
+9 -17
View File
@@ -1,6 +1,5 @@
import yaml
import os
import time
from abc import abstractmethod, ABC
from typing import Any, Dict, Optional, Tuple, List, Callable
@@ -10,7 +9,7 @@ from common.conversions import Conversions as CV
from common.kalman.simple_kalman import KF1D
from common.numpy_fast import clip
from common.realtime import DT_CTRL
from selfdrive.car import apply_hysteresis, gen_empty_fingerprint, scale_rot_inertia, scale_tire_stiffness
from selfdrive.car import apply_hysteresis, gen_empty_fingerprint, scale_rot_inertia, scale_tire_stiffness, STD_CARGO_KG
from selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX, get_friction
from selfdrive.controls.lib.events import Events
from selfdrive.controls.lib.vehicle_model import VehicleModel
@@ -100,16 +99,13 @@ class CarInterfaceBase(ABC):
ret = CarInterfaceBase.get_std_params(candidate)
ret = cls._get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs)
# Set common params using fields set by the car interface
# TODO: get actual value, for now starting with reasonable value for
# civic and scaling by mass and wheelbase
ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase)
# Vehicle mass is published curb weight plus assumed payload such as a human driver; notCars have no assumed payload
if not ret.notCar:
ret.mass = ret.mass + STD_CARGO_KG
# TODO: some car interfaces set stiffness factor
if ret.tireStiffnessFront == 0 or ret.tireStiffnessRear == 0:
# TODO: start from empirically derived lateral slip stiffness for the civic and scale by
# mass and CG position, so all cars will have approximately similar dyn behaviors
ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront)
# Set params dependent on values set by the car interface
ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase)
ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront, ret.tireStiffnessFactor)
return ret
@@ -152,6 +148,7 @@ class CarInterfaceBase(ABC):
ret.autoResumeSng = True # describes whether car can resume from a stop automatically
# standard ALC params
ret.tireStiffnessFactor = 1.0
ret.steerControlType = car.CarParams.SteerControlType.torque
ret.minSteerSpeed = 0.
ret.wheelSpeedFactor = 1.0
@@ -312,14 +309,9 @@ class RadarInterfaceBase(ABC):
self.rcp = None
self.pts = {}
self.delay = 0
self.radar_ts = CP.radarTimeStep
self.no_radar_sleep = 'NO_RADAR_SLEEP' in os.environ
def update(self, can_strings):
ret = car.RadarData.new_message()
if not self.no_radar_sleep:
time.sleep(self.radar_ts) # radard runs on RI updates
return ret
pass
class CarStateBase(ABC):
selfdrive/car/mazda/interface.py
+6 -11
View File
@@ -2,7 +2,7 @@
from cereal import car
from common.conversions import Conversions as CV
from selfdrive.car.mazda.values import CAR, LKAS_LIMITS
from selfdrive.car import STD_CARGO_KG, scale_tire_stiffness, get_safety_config
from selfdrive.car import get_safety_config
from selfdrive.car.interfaces import CarInterfaceBase
ButtonType = car.CarState.ButtonEvent.Type
@@ -20,24 +20,24 @@ class CarInterface(CarInterfaceBase):
ret.steerActuatorDelay = 0.1
ret.steerLimitTimer = 0.8
tire_stiffness_factor = 0.70 # not optimized yet
ret.tireStiffnessFactor = 0.70 # not optimized yet
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
if candidate in (CAR.CX5, CAR.CX5_2022):
ret.mass = 3655 * CV.LB_TO_KG + STD_CARGO_KG
ret.mass = 3655 * CV.LB_TO_KG
ret.wheelbase = 2.7
ret.steerRatio = 15.5
elif candidate in (CAR.CX9, CAR.CX9_2021):
ret.mass = 4217 * CV.LB_TO_KG + STD_CARGO_KG
ret.mass = 4217 * CV.LB_TO_KG
ret.wheelbase = 3.1
ret.steerRatio = 17.6
elif candidate == CAR.MAZDA3:
ret.mass = 2875 * CV.LB_TO_KG + STD_CARGO_KG
ret.mass = 2875 * CV.LB_TO_KG
ret.wheelbase = 2.7
ret.steerRatio = 14.0
elif candidate == CAR.MAZDA6:
ret.mass = 3443 * CV.LB_TO_KG + STD_CARGO_KG
ret.mass = 3443 * CV.LB_TO_KG
ret.wheelbase = 2.83
ret.steerRatio = 15.5
@@ -46,11 +46,6 @@ class CarInterface(CarInterfaceBase):
ret.centerToFront = ret.wheelbase * 0.41
# TODO: start from empirically derived lateral slip stiffness for the civic and scale by
# mass and CG position, so all cars will have approximately similar dyn behaviors
ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront,
tire_stiffness_factor=tire_stiffness_factor)
return ret
# returns a car.CarState
selfdrive/car/mock/interface.py
-2
View File
@@ -26,8 +26,6 @@ class CarInterface(CarInterfaceBase):
ret.wheelbase = 2.70
ret.centerToFront = ret.wheelbase * 0.5
ret.steerRatio = 13. # reasonable
ret.tireStiffnessFront = 1e6 # very stiff to neglect slip
ret.tireStiffnessRear = 1e6 # very stiff to neglect slip
return ret
selfdrive/car/nissan/interface.py
+4 -4
View File
@@ -1,6 +1,6 @@
#!/usr/bin/env python3
from cereal import car
from selfdrive.car import STD_CARGO_KG, get_safety_config
from selfdrive.car import get_safety_config
from selfdrive.car.interfaces import CarInterfaceBase
from selfdrive.car.nissan.values import CAR
@@ -22,17 +22,17 @@ class CarInterface(CarInterfaceBase):
ret.radarUnavailable = True
if candidate in (CAR.ROGUE, CAR.XTRAIL):
ret.mass = 1610 + STD_CARGO_KG
ret.mass = 1610
ret.wheelbase = 2.705
ret.centerToFront = ret.wheelbase * 0.44
elif candidate in (CAR.LEAF, CAR.LEAF_IC):
ret.mass = 1610 + STD_CARGO_KG
ret.mass = 1610
ret.wheelbase = 2.705
ret.centerToFront = ret.wheelbase * 0.44
elif candidate == CAR.ALTIMA:
# Altima has EPS on C-CAN unlike the others that have it on V-CAN
ret.safetyConfigs[0].safetyParam = 1 # EPS is on alternate bus
ret.mass = 1492 + STD_CARGO_KG
ret.mass = 1492
ret.wheelbase = 2.824
ret.centerToFront = ret.wheelbase * 0.44
selfdrive/car/subaru/carcontroller.py
+32 -6
View File
@@ -1,3 +1,4 @@
from common.numpy_fast import clip, interp
from opendbc.can.packer import CANPacker
from selfdrive.car import apply_driver_steer_torque_limits
from selfdrive.car.subaru import subarucan
@@ -42,6 +43,21 @@ class CarController:
self.apply_steer_last = apply_steer
# *** longitudinal ***
if CC.longActive:
apply_throttle = int(round(interp(actuators.accel, CarControllerParams.THROTTLE_LOOKUP_BP, CarControllerParams.THROTTLE_LOOKUP_V)))
apply_rpm = int(round(interp(actuators.accel, CarControllerParams.RPM_LOOKUP_BP, CarControllerParams.RPM_LOOKUP_V)))
apply_brake = int(round(interp(actuators.accel, CarControllerParams.BRAKE_LOOKUP_BP, CarControllerParams.BRAKE_LOOKUP_V)))
# limit min and max values
cruise_throttle = clip(apply_throttle, CarControllerParams.THROTTLE_MIN, CarControllerParams.THROTTLE_MAX)
cruise_rpm = clip(apply_rpm, CarControllerParams.RPM_MIN, CarControllerParams.RPM_MAX)
cruise_brake = clip(apply_brake, CarControllerParams.BRAKE_MIN, CarControllerParams.BRAKE_MAX)
else:
cruise_throttle = CarControllerParams.THROTTLE_INACTIVE
cruise_rpm = CarControllerParams.RPM_INACTIVE
cruise_brake = CarControllerParams.BRAKE_MIN
# *** alerts and pcm cancel ***
if self.CP.carFingerprint in PREGLOBAL_CARS:
@@ -64,13 +80,9 @@ class CarController:
can_sends.append(subarucan.create_preglobal_es_distance(self.packer, cruise_button, CS.es_distance_msg))
else:
if pcm_cancel_cmd and (self.frame - self.last_cancel_frame) > 0.2:
bus = CanBus.alt if self.CP.carFingerprint in GLOBAL_GEN2 else CanBus.main
can_sends.append(subarucan.create_es_distance(self.packer, CS.es_distance_msg, bus, pcm_cancel_cmd))
self.last_cancel_frame = self.frame
if self.frame % 10 == 0:
can_sends.append(subarucan.create_es_dashstatus(self.packer, CS.es_dashstatus_msg))
can_sends.append(subarucan.create_es_dashstatus(self.packer, CS.es_dashstatus_msg, CC.enabled, self.CP.openpilotLongitudinalControl,
CC.longActive, hud_control.leadVisible))
can_sends.append(subarucan.create_es_lkas_state(self.packer, CS.es_lkas_state_msg, CC.enabled, hud_control.visualAlert,
hud_control.leftLaneVisible, hud_control.rightLaneVisible,
@@ -79,6 +91,20 @@ class CarController:
if self.CP.flags & SubaruFlags.SEND_INFOTAINMENT:
can_sends.append(subarucan.create_es_infotainment(self.packer, CS.es_infotainment_msg, hud_control.visualAlert))
if self.CP.openpilotLongitudinalControl:
if self.frame % 5 == 0:
can_sends.append(subarucan.create_es_status(self.packer, CS.es_status_msg, self.CP.openpilotLongitudinalControl, CC.longActive, cruise_rpm))
can_sends.append(subarucan.create_es_brake(self.packer, CS.es_brake_msg, CC.enabled, cruise_brake))
can_sends.append(subarucan.create_es_distance(self.packer, CS.es_distance_msg, 0, pcm_cancel_cmd,
self.CP.openpilotLongitudinalControl, cruise_brake > 0, cruise_throttle))
else:
if pcm_cancel_cmd and (self.frame - self.last_cancel_frame) > 0.2:
bus = CanBus.alt if self.CP.carFingerprint in GLOBAL_GEN2 else CanBus.main
can_sends.append(subarucan.create_es_distance(self.packer, CS.es_distance_msg, bus, pcm_cancel_cmd))
self.last_cancel_frame = self.frame
new_actuators = actuators.copy()
new_actuators.steer = self.apply_steer_last / self.p.STEER_MAX
new_actuators.steerOutputCan = self.apply_steer_last
selfdrive/car/subaru/carstate.py
+16
View File
@@ -5,6 +5,7 @@ from common.conversions import Conversions as CV
from selfdrive.car.interfaces import CarStateBase
from opendbc.can.parser import CANParser
from selfdrive.car.subaru.values import DBC, CAR, GLOBAL_GEN2, PREGLOBAL_CARS, CanBus, SubaruFlags
from selfdrive.car import CanSignalRateCalculator
class CarState(CarStateBase):
@@ -13,6 +14,8 @@ class CarState(CarStateBase):
can_define = CANDefine(DBC[CP.carFingerprint]["pt"])
self.shifter_values = can_define.dv["Transmission"]["Gear"]
self.angle_rate_calulator = CanSignalRateCalculator(50)
def update(self, cp, cp_cam, cp_body):
ret = car.CarState.new_message()
@@ -47,6 +50,11 @@ class CarState(CarStateBase):
ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(can_gear, None))
ret.steeringAngleDeg = cp.vl["Steering_Torque"]["Steering_Angle"]
if self.car_fingerprint not in PREGLOBAL_CARS:
# ideally we get this from the car, but unclear if it exists. diagnostic software doesn't even have it
ret.steeringRateDeg = self.angle_rate_calulator.update(ret.steeringAngleDeg, cp.vl["Steering_Torque"]["COUNTER"])
ret.steeringTorque = cp.vl["Steering_Torque"]["Steer_Torque_Sensor"]
ret.steeringTorqueEps = cp.vl["Steering_Torque"]["Steer_Torque_Output"]
@@ -83,6 +91,12 @@ class CarState(CarStateBase):
ret.stockAeb = (cp_es_distance.vl["ES_Brake"]["AEB_Status"] == 8) and \
(cp_es_distance.vl["ES_Brake"]["Brake_Pressure"] != 0)
self.es_lkas_state_msg = copy.copy(cp_cam.vl["ES_LKAS_State"])
cp_es_brake = cp_body if self.car_fingerprint in GLOBAL_GEN2 else cp_cam
self.es_brake_msg = copy.copy(cp_es_brake.vl["ES_Brake"])
cp_es_status = cp_body if self.car_fingerprint in GLOBAL_GEN2 else cp_cam
self.es_status_msg = copy.copy(cp_es_status.vl["ES_Status"])
self.cruise_control_msg = copy.copy(cp_cruise.vl["CruiseControl"])
self.brake_status_msg = copy.copy(cp_brakes.vl["Brake_Status"])
self.es_distance_msg = copy.copy(cp_es_distance.vl["ES_Distance"])
self.es_dashstatus_msg = copy.copy(cp_cam.vl["ES_DashStatus"])
@@ -106,6 +120,8 @@ class CarState(CarStateBase):
messages = [
("ES_Brake", 20),
("ES_Distance", 20),
("ES_Status", 20),
("ES_Brake", 20),
]
return messages
selfdrive/car/subaru/interface.py
+21 -9
View File
@@ -1,7 +1,7 @@
#!/usr/bin/env python3
from cereal import car
from panda import Panda
from selfdrive.car import STD_CARGO_KG, get_safety_config
from selfdrive.car import get_safety_config
from selfdrive.car.interfaces import CarInterfaceBase
from selfdrive.car.subaru.values import CAR, LKAS_ANGLE, GLOBAL_GEN2, PREGLOBAL_CARS, SubaruFlags
@@ -37,7 +37,7 @@ class CarInterface(CarInterfaceBase):
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
if candidate == CAR.ASCENT:
ret.mass = 2031. + STD_CARGO_KG
ret.mass = 2031.
ret.wheelbase = 2.89
ret.centerToFront = ret.wheelbase * 0.5
ret.steerRatio = 13.5
@@ -48,7 +48,7 @@ class CarInterface(CarInterfaceBase):
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.0025, 0.1], [0.00025, 0.01]]
elif candidate == CAR.IMPREZA:
ret.mass = 1568. + STD_CARGO_KG
ret.mass = 1568.
ret.wheelbase = 2.67
ret.centerToFront = ret.wheelbase * 0.5
ret.steerRatio = 15
@@ -59,7 +59,7 @@ class CarInterface(CarInterfaceBase):
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2, 0.3], [0.02, 0.03]]
elif candidate == CAR.IMPREZA_2020:
ret.mass = 1480. + STD_CARGO_KG
ret.mass = 1480.
ret.wheelbase = 2.67
ret.centerToFront = ret.wheelbase * 0.5
ret.steerRatio = 17 # learned, 14 stock
@@ -69,7 +69,7 @@ class CarInterface(CarInterfaceBase):
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.045, 0.042, 0.20], [0.04, 0.035, 0.045]]
elif candidate in (CAR.FORESTER, CAR.FORESTER_2022):
ret.mass = 1568. + STD_CARGO_KG
ret.mass = 1568.
ret.wheelbase = 2.67
ret.centerToFront = ret.wheelbase * 0.5
ret.steerRatio = 17 # learned, 14 stock
@@ -79,7 +79,7 @@ class CarInterface(CarInterfaceBase):
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.01, 0.065, 0.2], [0.001, 0.015, 0.025]]
elif candidate in (CAR.OUTBACK, CAR.LEGACY, CAR.OUTBACK_2023):
ret.mass = 1568. + STD_CARGO_KG
ret.mass = 1568.
ret.wheelbase = 2.67
ret.centerToFront = ret.wheelbase * 0.5
ret.steerRatio = 17
@@ -87,26 +87,38 @@ class CarInterface(CarInterfaceBase):
elif candidate in (CAR.FORESTER_PREGLOBAL, CAR.OUTBACK_PREGLOBAL_2018):
ret.safetyConfigs[0].safetyParam = 1 # Outback 2018-2019 and Forester have reversed driver torque signal
ret.mass = 1568 + STD_CARGO_KG
ret.mass = 1568
ret.wheelbase = 2.67
ret.centerToFront = ret.wheelbase * 0.5
ret.steerRatio = 20 # learned, 14 stock
elif candidate == CAR.LEGACY_PREGLOBAL:
ret.mass = 1568 + STD_CARGO_KG
ret.mass = 1568
ret.wheelbase = 2.67
ret.centerToFront = ret.wheelbase * 0.5
ret.steerRatio = 12.5 # 14.5 stock
ret.steerActuatorDelay = 0.15
elif candidate == CAR.OUTBACK_PREGLOBAL:
ret.mass = 1568 + STD_CARGO_KG
ret.mass = 1568
ret.wheelbase = 2.67
ret.centerToFront = ret.wheelbase * 0.5
ret.steerRatio = 20 # learned, 14 stock
else:
raise ValueError(f"unknown car: {candidate}")
#ret.experimentalLongitudinalAvailable = candidate not in (GLOBAL_GEN2 | PREGLOBAL_CARS | LKAS_ANGLE)
ret.openpilotLongitudinalControl = experimental_long and ret.experimentalLongitudinalAvailable
if ret.openpilotLongitudinalControl:
ret.longitudinalTuning.kpBP = [0., 5., 35.]
ret.longitudinalTuning.kpV = [0.8, 1.0, 1.5]
ret.longitudinalTuning.kiBP = [0., 35.]
ret.longitudinalTuning.kiV = [0.54, 0.36]
ret.stoppingControl = True
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_SUBARU_LONG
return ret
# returns a car.CarState
selfdrive/car/subaru/subarucan.py
+70 -4
View File
@@ -16,8 +16,7 @@ def create_steering_control(packer, apply_steer, steer_req):
def create_steering_status(packer):
return packer.make_can_msg("ES_LKAS_State", 0, {})
def create_es_distance(packer, es_distance_msg, bus, pcm_cancel_cmd):
def create_es_distance(packer, es_distance_msg, bus, pcm_cancel_cmd, long_enabled = False, brake_cmd = False, cruise_throttle = 0):
values = {s: es_distance_msg[s] for s in [
"CHECKSUM",
"COUNTER",
@@ -41,8 +40,20 @@ def create_es_distance(packer, es_distance_msg, bus, pcm_cancel_cmd):
"Signal6",
]}
values["COUNTER"] = (values["COUNTER"] + 1) % 0x10
if long_enabled:
values["Cruise_Throttle"] = cruise_throttle
# Do not disable openpilot on Eyesight Soft Disable, if openpilot is controlling long
values["Cruise_Soft_Disable"] = 0
if brake_cmd:
values["Cruise_Brake_Active"] = 1
if pcm_cancel_cmd:
values["Cruise_Cancel"] = 1
values["Cruise_Throttle"] = 1818 # inactive throttle
return packer.make_can_msg("ES_Distance", bus, values)
@@ -105,8 +116,7 @@ def create_es_lkas_state(packer, es_lkas_state_msg, enabled, visual_alert, left_
return packer.make_can_msg("ES_LKAS_State", CanBus.main, values)
def create_es_dashstatus(packer, dashstatus_msg):
def create_es_dashstatus(packer, dashstatus_msg, enabled, long_enabled, long_active, lead_visible):
values = {s: dashstatus_msg[s] for s in [
"CHECKSUM",
"COUNTER",
@@ -137,12 +147,68 @@ def create_es_dashstatus(packer, dashstatus_msg):
"Cruise_State",
]}
if enabled and long_active:
values["Cruise_State"] = 0
values["Cruise_Activated"] = 1
values["Cruise_Disengaged"] = 0
values["Car_Follow"] = int(lead_visible)
if long_enabled:
values["PCB_Off"] = 1 # AEB is not presevered, so show the PCB_Off on dash
# Filter stock LKAS disabled and Keep hands on steering wheel OFF alerts
if values["LKAS_State_Msg"] in (2, 3):
values["LKAS_State_Msg"] = 0
return packer.make_can_msg("ES_DashStatus", CanBus.main, values)
def create_es_brake(packer, es_brake_msg, enabled, brake_value):
values = {s: es_brake_msg[s] for s in [
"CHECKSUM",
"COUNTER",
"Signal1",
"Brake_Pressure",
"AEB_Status",
"Cruise_Brake_Lights",
"Cruise_Brake_Fault",
"Cruise_Brake_Active",
"Cruise_Activated",
"Signal3",
]}
if enabled:
values["Cruise_Activated"] = 1
values["Brake_Pressure"] = brake_value
if brake_value > 0:
values["Cruise_Brake_Active"] = 1
values["Cruise_Brake_Lights"] = 1 if brake_value >= 70 else 0
return packer.make_can_msg("ES_Brake", CanBus.main, values)
def create_es_status(packer, es_status_msg, long_enabled, long_active, cruise_rpm):
values = {s: es_status_msg[s] for s in [
"CHECKSUM",
"COUNTER",
"Signal1",
"Cruise_Fault",
"Cruise_RPM",
"Signal2",
"Cruise_Activated",
"Brake_Lights",
"Cruise_Hold",
"Signal3",
]}
if long_enabled:
values["Cruise_RPM"] = cruise_rpm
if long_active:
values["Cruise_Activated"] = 1
return packer.make_can_msg("ES_Status", CanBus.main, values)
def create_es_infotainment(packer, es_infotainment_msg, visual_alert):
# Filter stock LKAS disabled and Keep hands on steering wheel OFF alerts
selfdrive/car/subaru/values.py
+24
View File
@@ -29,6 +29,29 @@ class CarControllerParams:
else:
self.STEER_MAX = 2047
THROTTLE_MIN = 808
THROTTLE_MAX = 3400
THROTTLE_INACTIVE = 1818 # corresponds to zero acceleration
THROTTLE_ENGINE_BRAKE = 808 # while braking, eyesight sets throttle to this, probably for engine braking
BRAKE_MIN = 0
BRAKE_MAX = 600 # about -3.5m/s2 from testing
RPM_MIN = 0
RPM_MAX = 2400
RPM_INACTIVE = 600 # a good base rpm for zero acceleration
THROTTLE_LOOKUP_BP = [0, 1]
THROTTLE_LOOKUP_V = [THROTTLE_INACTIVE, THROTTLE_MAX]
RPM_LOOKUP_BP = [0, 1]
RPM_LOOKUP_V = [RPM_INACTIVE, RPM_MAX]
BRAKE_LOOKUP_BP = [-1, 0]
BRAKE_LOOKUP_V = [BRAKE_MAX, BRAKE_MIN]
class SubaruFlags(IntFlag):
SEND_INFOTAINMENT = 1
@@ -288,6 +311,7 @@ FW_VERSIONS = {
b'\xe6!fp\x07',
b'\xf3"fp\x07',
b'\xe6"f0\x07',
b'\xe6"fp\x07',
],
(Ecu.transmission, 0x7e1, None): [
b'\xe6\xf5\004\000\000',
selfdrive/car/tesla/interface.py
+2 -2
View File
@@ -2,7 +2,7 @@
from cereal import car
from panda import Panda
from selfdrive.car.tesla.values import CANBUS, CAR
from selfdrive.car import STD_CARGO_KG, get_safety_config
from selfdrive.car import get_safety_config
from selfdrive.car.interfaces import CarInterfaceBase
@@ -42,7 +42,7 @@ class CarInterface(CarInterfaceBase):
ret.steerActuatorDelay = 0.25
if candidate in (CAR.AP2_MODELS, CAR.AP1_MODELS):
ret.mass = 2100. + STD_CARGO_KG
ret.mass = 2100.
ret.wheelbase = 2.959
ret.centerToFront = ret.wheelbase * 0.5
ret.steerRatio = 15.0
selfdrive/car/tesla/radar_interface.py
+1 -1
View File
@@ -36,7 +36,7 @@ class RadarInterface(RadarInterfaceBase):
def update(self, can_strings):
if self.rcp is None:
return super().update(None)
return None
values = self.rcp.update_strings(can_strings)
self.updated_messages.update(values)
selfdrive/car/tests/routes.py
+7 -3
View File
@@ -1,5 +1,5 @@
#!/usr/bin/env python3
from collections import namedtuple
from typing import NamedTuple, Optional
from selfdrive.car.chrysler.values import CAR as CHRYSLER
from selfdrive.car.gm.values import CAR as GM
@@ -30,7 +30,12 @@ non_tested_cars = [
TOYOTA.RAV4H_TSS2_2023,
]
CarTestRoute = namedtuple('CarTestRoute', ['route', 'car_model', 'segment'], defaults=(None,))
class CarTestRoute(NamedTuple):
route: str
car_model: Optional[str]
segment: Optional[int] = None
routes = [
CarTestRoute("efdf9af95e71cd84|2022-05-13--19-03-31", COMMA.BODY),
@@ -166,7 +171,6 @@ routes = [
CarTestRoute("5f5afb36036506e4|2019-05-14--02-09-54", TOYOTA.COROLLA_TSS2),
CarTestRoute("5ceff72287a5c86c|2019-10-19--10-59-02", TOYOTA.COROLLAH_TSS2),
CarTestRoute("d2525c22173da58b|2021-04-25--16-47-04", TOYOTA.PRIUS),
CarTestRoute("b14c5b4742e6fc85|2020-07-28--19-50-11", TOYOTA.RAV4),
CarTestRoute("32a7df20486b0f70|2020-02-06--16-06-50", TOYOTA.RAV4H),
CarTestRoute("cdf2f7de565d40ae|2019-04-25--03-53-41", TOYOTA.RAV4_TSS2),
CarTestRoute("a5c341bb250ca2f0|2022-05-18--16-05-17", TOYOTA.RAV4_TSS2_2022),
selfdrive/car/tests/test_can_fingerprint.py
Executable
+30
View File
@@ -0,0 +1,30 @@
#!/usr/bin/env python3
from parameterized import parameterized
import unittest
from cereal import log, messaging
from selfdrive.car.car_helpers import can_fingerprint
from selfdrive.car.fingerprints import _FINGERPRINTS as FINGERPRINTS
class TestCanFingerprint(unittest.TestCase):
@parameterized.expand([(c, f) for c, f in FINGERPRINTS.items()])
def test_can_fingerprint(self, car_model, fingerprints):
"""Tests online fingerprinting function on offline fingerprints"""
for fingerprint in fingerprints: # can have multiple fingerprints for each platform
can = messaging.new_message('can', 1)
can.can = [log.CanData(address=address, dat=b'\x00' * length, src=src)
for address, length in fingerprint.items() for src in (0, 1)]
fingerprint_iter = iter([can])
empty_can = messaging.new_message('can', 0)
car_fingerprint, finger = can_fingerprint(lambda: next(fingerprint_iter, empty_can)) # noqa: B023
self.assertEqual(car_fingerprint, car_model)
self.assertEqual(finger[0], fingerprint)
self.assertEqual(finger[1], fingerprint)
if __name__ == "__main__":
unittest.main()
selfdrive/car/tests/test_fingerprints.py
+1 -1
View File
@@ -70,7 +70,7 @@ if __name__ == "__main__":
for brand in fingerprints:
for car in fingerprints[brand]:
fingerprints_flat += fingerprints[brand][car]
for i in range(len(fingerprints[brand][car])):
for _ in range(len(fingerprints[brand][car])):
car_names.append(car)
brand_names.append(brand)
selfdrive/car/tests/test_models.py
+46 -16
View File
@@ -1,5 +1,6 @@
#!/usr/bin/env python3
# pylint: disable=E1101
import capnp
import os
import importlib
import unittest
@@ -11,7 +12,7 @@ from cereal import log, car
from common.basedir import BASEDIR
from common.realtime import DT_CTRL
from selfdrive.car.fingerprints import all_known_cars
from selfdrive.car.car_helpers import interfaces
from selfdrive.car.car_helpers import FRAME_FINGERPRINT, interfaces
from selfdrive.car.gm.values import CAR as GM
from selfdrive.car.honda.values import CAR as HONDA, HONDA_BOSCH
from selfdrive.car.hyundai.values import CAR as HYUNDAI
@@ -27,6 +28,7 @@ PandaType = log.PandaState.PandaType
NUM_JOBS = int(os.environ.get("NUM_JOBS", "1"))
JOB_ID = int(os.environ.get("JOB_ID", "0"))
INTERNAL_SEG_LIST = os.environ.get("INTERNAL_SEG_LIST", "")
INTERNAL_SEG_CNT = int(os.environ.get("INTERNAL_SEG_CNT", "0"))
ignore_addr_checks_valid = [
GM.BUICK_REGAL,
@@ -34,9 +36,9 @@ ignore_addr_checks_valid = [
]
def get_test_cases():
def get_test_cases() -> List[Tuple[str, Optional[CarTestRoute]]]:
# build list of test cases
test_cases: List[Tuple[str, Optional[CarTestRoute]]] = []
test_cases = []
if not len(INTERNAL_SEG_LIST):
routes_by_car = defaultdict(set)
for r in routes:
@@ -48,11 +50,14 @@ def get_test_cases():
else:
with open(os.path.join(BASEDIR, INTERNAL_SEG_LIST), "r") as f:
seg_list = iter(f.read().splitlines())
seg_list = f.read().splitlines()
for platform in seg_list:
cnt = INTERNAL_SEG_CNT or len(seg_list)
seg_list_iter = iter(seg_list[:cnt])
for platform in seg_list_iter:
platform = platform[2:] # get rid of comment
segment_name = SegmentName(next(seg_list))
segment_name = SegmentName(next(seg_list_iter))
test_cases.append((platform, CarTestRoute(segment_name.route_name.canonical_name, platform,
segment=segment_name.segment_num)))
return test_cases
@@ -62,9 +67,11 @@ SKIP_ENV_VAR = "SKIP_LONG_TESTS"
class TestCarModelBase(unittest.TestCase):
car_model = None
test_route = None
ci = True
car_model: Optional[str] = None
test_route: Optional[CarTestRoute] = None
ci: bool = True
can_msgs: List[capnp.lib.capnp._DynamicStructReader]
@unittest.skipIf(SKIP_ENV_VAR in os.environ, f"Long running test skipped. Unset {SKIP_ENV_VAR} to run")
@classmethod
@@ -82,7 +89,6 @@ class TestCarModelBase(unittest.TestCase):
raise unittest.SkipTest
raise Exception(f"missing test route for {cls.car_model}")
experimental_long = False
test_segs = (2, 1, 0)
if cls.test_route.segment is not None:
test_segs = (cls.test_route.segment,)
@@ -102,24 +108,38 @@ class TestCarModelBase(unittest.TestCase):
car_fw = []
can_msgs = []
fingerprint = defaultdict(dict)
experimental_long = False
enabled_toggle = True
dashcam_only = False
for msg in lr:
if msg.which() == "can":
for m in msg.can:
if m.src < 64:
fingerprint[m.src][m.address] = len(m.dat)
can_msgs.append(msg)
if len(can_msgs) <= FRAME_FINGERPRINT:
for m in msg.can:
if m.src < 64:
fingerprint[m.src][m.address] = len(m.dat)
elif msg.which() == "carParams":
car_fw = msg.carParams.carFw
dashcam_only = msg.carParams.dashcamOnly
if msg.carParams.openpilotLongitudinalControl:
experimental_long = True
if cls.car_model is None and not cls.ci:
cls.car_model = msg.carParams.carFingerprint
elif msg.which() == 'initData':
for param in msg.initData.params.entries:
if param.key == 'OpenpilotEnabledToggle':
enabled_toggle = param.value.strip(b'\x00') == b'1'
if len(can_msgs) > int(50 / DT_CTRL):
break
else:
raise Exception(f"Route: {repr(cls.test_route.route)} with segments: {test_segs} not found or no CAN msgs found. Is it uploaded?")
# if relay is expected to be open in the route
cls.openpilot_enabled = enabled_toggle and not dashcam_only
cls.can_msgs = sorted(can_msgs, key=lambda msg: msg.logMonoTime)
cls.CarInterface, cls.CarController, cls.CarState = interfaces[cls.car_model]
@@ -219,6 +239,14 @@ class TestCarModelBase(unittest.TestCase):
self.safety.safety_tick_current_rx_checks()
if t > 1e6:
self.assertTrue(self.safety.addr_checks_valid())
# No need to check relay malfunction on disabled routes (relay closed) or for reasonable fingerprinting time
# TODO: detect when relay has flipped to properly check relay malfunction
if self.openpilot_enabled and t > 5e6:
self.assertFalse(self.safety.get_relay_malfunction())
else:
self.safety.set_relay_malfunction(False)
self.assertFalse(len(failed_addrs), f"panda safety RX check failed: {failed_addrs}")
def test_panda_safety_tx_cases(self, data=None):
@@ -294,9 +322,7 @@ class TestCarModelBase(unittest.TestCase):
# TODO: check rest of panda's carstate (steering, ACC main on, etc.)
checks['gasPressed'] += CS.gasPressed != self.safety.get_gas_pressed_prev()
if self.CP.carName not in ("hyundai", "body"):
# TODO: fix standstill mismatches for other makes
checks['standstill'] += CS.standstill == self.safety.get_vehicle_moving()
checks['standstill'] += CS.standstill == self.safety.get_vehicle_moving()
# TODO: remove this exception once this mismatch is resolved
brake_pressed = CS.brakePressed
@@ -316,6 +342,10 @@ class TestCarModelBase(unittest.TestCase):
checks['controlsAllowed'] += not self.safety.get_controls_allowed()
else:
checks['controlsAllowed'] += not CS.cruiseState.enabled and self.safety.get_controls_allowed()
# TODO: fix notCar mismatch
if not self.CP.notCar:
checks['cruiseState'] += CS.cruiseState.enabled != self.safety.get_cruise_engaged_prev()
else:
# Check for enable events on rising edge of controls allowed
button_enable = any(evt.enable for evt in CS.events)
selfdrive/car/tests/test_models_segs.txt
+2 -2
View File
@@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:636df10e41e8c6668678a3aa7098afad8c160ea7b75c5a0473f6709db1c702dc
size 39373
oid sha256:b93de5a9a259934896d8573d9b0f696289b88693908968009fe144e2e10a3355
size 127597
selfdrive/car/toyota/interface.py
+39 -45
View File
@@ -4,7 +4,7 @@ from common.conversions import Conversions as CV
from panda import Panda
from selfdrive.car.toyota.values import Ecu, CAR, DBC, ToyotaFlags, CarControllerParams, TSS2_CAR, RADAR_ACC_CAR, NO_DSU_CAR, \
MIN_ACC_SPEED, EPS_SCALE, EV_HYBRID_CAR, UNSUPPORTED_DSU_CAR, NO_STOP_TIMER_CAR, ANGLE_CONTROL_CAR
from selfdrive.car import STD_CARGO_KG, scale_tire_stiffness, get_safety_config
from selfdrive.car import get_safety_config
from selfdrive.car.interfaces import CarInterfaceBase
EventName = car.CarEvent.EventName
@@ -48,8 +48,8 @@ class CarInterface(CarInterfaceBase):
stop_and_go = True
ret.wheelbase = 2.70
ret.steerRatio = 15.74 # unknown end-to-end spec
tire_stiffness_factor = 0.6371 # hand-tune
ret.mass = 3045. * CV.LB_TO_KG + STD_CARGO_KG
ret.tireStiffnessFactor = 0.6371 # hand-tune
ret.mass = 3045. * CV.LB_TO_KG
# Only give steer angle deadzone to for bad angle sensor prius
for fw in car_fw:
if fw.ecu == "eps" and not fw.fwVersion == b'8965B47060\x00\x00\x00\x00\x00\x00':
@@ -60,50 +60,50 @@ class CarInterface(CarInterfaceBase):
stop_and_go = True
ret.wheelbase = 2.78
ret.steerRatio = 17.4
tire_stiffness_factor = 0.5533
ret.mass = 3340. * CV.LB_TO_KG + STD_CARGO_KG
ret.tireStiffnessFactor = 0.5533
ret.mass = 3340. * CV.LB_TO_KG
elif candidate in (CAR.RAV4, CAR.RAV4H):
stop_and_go = True if (candidate in CAR.RAV4H) else False
ret.wheelbase = 2.65
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
ret.tireStiffnessFactor = 0.5533
ret.mass = 3650. * CV.LB_TO_KG # mean between normal and hybrid
elif candidate == CAR.COROLLA:
ret.wheelbase = 2.70
ret.steerRatio = 18.27
tire_stiffness_factor = 0.444 # not optimized yet
ret.mass = 2860. * CV.LB_TO_KG + STD_CARGO_KG # mean between normal and hybrid
ret.tireStiffnessFactor = 0.444 # not optimized yet
ret.mass = 2860. * CV.LB_TO_KG # mean between normal and hybrid
elif candidate in (CAR.LEXUS_RX, CAR.LEXUS_RXH, CAR.LEXUS_RX_TSS2, CAR.LEXUS_RXH_TSS2):
stop_and_go = True
ret.wheelbase = 2.79
ret.steerRatio = 16. # 14.8 is spec end-to-end
ret.wheelSpeedFactor = 1.035
tire_stiffness_factor = 0.5533
ret.mass = 4481. * CV.LB_TO_KG + STD_CARGO_KG # mean between min and max
ret.tireStiffnessFactor = 0.5533
ret.mass = 4481. * CV.LB_TO_KG # mean between min and max
elif candidate in (CAR.CHR, CAR.CHRH, CAR.CHR_TSS2, CAR.CHRH_TSS2):
stop_and_go = True
ret.wheelbase = 2.63906
ret.steerRatio = 13.6
tire_stiffness_factor = 0.7933
ret.mass = 3300. * CV.LB_TO_KG + STD_CARGO_KG
ret.tireStiffnessFactor = 0.7933
ret.mass = 3300. * CV.LB_TO_KG
elif candidate in (CAR.CAMRY, CAR.CAMRYH, CAR.CAMRY_TSS2, CAR.CAMRYH_TSS2):
stop_and_go = True
ret.wheelbase = 2.82448
ret.steerRatio = 13.7
tire_stiffness_factor = 0.7933
ret.mass = 3400. * CV.LB_TO_KG + STD_CARGO_KG # mean between normal and hybrid
ret.tireStiffnessFactor = 0.7933
ret.mass = 3400. * CV.LB_TO_KG # mean between normal and hybrid
elif candidate in (CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.HIGHLANDER_TSS2, CAR.HIGHLANDERH_TSS2):
stop_and_go = True
ret.wheelbase = 2.8194 # average of 109.8 and 112.2 in
ret.steerRatio = 16.0
tire_stiffness_factor = 0.8
ret.mass = 4516. * CV.LB_TO_KG + STD_CARGO_KG # mean between normal and hybrid
ret.tireStiffnessFactor = 0.8
ret.mass = 4516. * CV.LB_TO_KG # mean between normal and hybrid
elif candidate in (CAR.AVALON, CAR.AVALON_2019, CAR.AVALONH_2019, CAR.AVALON_TSS2, CAR.AVALONH_TSS2):
# starting from 2019, all Avalon variants have stop and go
@@ -111,15 +111,15 @@ class CarInterface(CarInterfaceBase):
stop_and_go = candidate != CAR.AVALON
ret.wheelbase = 2.82
ret.steerRatio = 14.8 # Found at https://pressroom.toyota.com/releases/2016+avalon+product+specs.download
tire_stiffness_factor = 0.7983
ret.mass = 3505. * CV.LB_TO_KG + STD_CARGO_KG # mean between normal and hybrid
ret.tireStiffnessFactor = 0.7983
ret.mass = 3505. * CV.LB_TO_KG # mean between normal and hybrid
elif candidate in (CAR.RAV4_TSS2, CAR.RAV4_TSS2_2022, CAR.RAV4H_TSS2, CAR.RAV4H_TSS2_2022,
CAR.RAV4_TSS2_2023, CAR.RAV4H_TSS2_2023):
ret.wheelbase = 2.68986
ret.steerRatio = 14.3
tire_stiffness_factor = 0.7933
ret.mass = 3585. * CV.LB_TO_KG + STD_CARGO_KG # Average between ICE and Hybrid
ret.tireStiffnessFactor = 0.7933
ret.mass = 3585. * CV.LB_TO_KG # Average between ICE and Hybrid
ret.lateralTuning.init('pid')
ret.lateralTuning.pid.kiBP = [0.0]
ret.lateralTuning.pid.kpBP = [0.0]
@@ -139,70 +139,64 @@ class CarInterface(CarInterfaceBase):
elif candidate in (CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2):
ret.wheelbase = 2.67 # Average between 2.70 for sedan and 2.64 for hatchback
ret.steerRatio = 13.9
tire_stiffness_factor = 0.444 # not optimized yet
ret.mass = 3060. * CV.LB_TO_KG + STD_CARGO_KG
ret.tireStiffnessFactor = 0.444 # not optimized yet
ret.mass = 3060. * CV.LB_TO_KG
elif candidate in (CAR.LEXUS_ES, CAR.LEXUS_ESH, CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2):
if candidate not in (CAR.LEXUS_ES,): # TODO: LEXUS_ES may have sng
stop_and_go = True
ret.wheelbase = 2.8702
ret.steerRatio = 16.0 # not optimized
tire_stiffness_factor = 0.444 # not optimized yet
ret.mass = 3677. * CV.LB_TO_KG + STD_CARGO_KG # mean between min and max
ret.tireStiffnessFactor = 0.444 # not optimized yet
ret.mass = 3677. * CV.LB_TO_KG # mean between min and max
elif candidate == CAR.SIENNA:
stop_and_go = True
ret.wheelbase = 3.03
ret.steerRatio = 15.5
tire_stiffness_factor = 0.444
ret.mass = 4590. * CV.LB_TO_KG + STD_CARGO_KG
ret.tireStiffnessFactor = 0.444
ret.mass = 4590. * CV.LB_TO_KG
elif candidate in (CAR.LEXUS_IS, CAR.LEXUS_IS_TSS2, CAR.LEXUS_RC):
ret.wheelbase = 2.79908
ret.steerRatio = 13.3
tire_stiffness_factor = 0.444
ret.mass = 3736.8 * CV.LB_TO_KG + STD_CARGO_KG
ret.tireStiffnessFactor = 0.444
ret.mass = 3736.8 * CV.LB_TO_KG
elif candidate == CAR.LEXUS_CTH:
stop_and_go = True
ret.wheelbase = 2.60
ret.steerRatio = 18.6
tire_stiffness_factor = 0.517
ret.mass = 3108 * CV.LB_TO_KG + STD_CARGO_KG # mean between min and max
ret.tireStiffnessFactor = 0.517
ret.mass = 3108 * CV.LB_TO_KG # mean between min and max
elif candidate in (CAR.LEXUS_NX, CAR.LEXUS_NXH, CAR.LEXUS_NX_TSS2, CAR.LEXUS_NXH_TSS2):
stop_and_go = True
ret.wheelbase = 2.66
ret.steerRatio = 14.7
tire_stiffness_factor = 0.444 # not optimized yet
ret.mass = 4070 * CV.LB_TO_KG + STD_CARGO_KG
ret.tireStiffnessFactor = 0.444 # not optimized yet
ret.mass = 4070 * CV.LB_TO_KG
elif candidate == CAR.PRIUS_TSS2:
ret.wheelbase = 2.70002 # from toyota online sepc.
ret.steerRatio = 13.4 # True steerRatio from older prius
tire_stiffness_factor = 0.6371 # hand-tune
ret.mass = 3115. * CV.LB_TO_KG + STD_CARGO_KG
ret.tireStiffnessFactor = 0.6371 # hand-tune
ret.mass = 3115. * CV.LB_TO_KG
elif candidate == CAR.MIRAI:
stop_and_go = True
ret.wheelbase = 2.91
ret.steerRatio = 14.8
tire_stiffness_factor = 0.8
ret.mass = 4300. * CV.LB_TO_KG + STD_CARGO_KG
ret.tireStiffnessFactor = 0.8
ret.mass = 4300. * CV.LB_TO_KG
elif candidate in (CAR.ALPHARD_TSS2, CAR.ALPHARDH_TSS2):
ret.wheelbase = 3.00
ret.steerRatio = 14.2
tire_stiffness_factor = 0.444
ret.mass = 4305. * CV.LB_TO_KG + STD_CARGO_KG
ret.tireStiffnessFactor = 0.444
ret.mass = 4305. * CV.LB_TO_KG
ret.centerToFront = ret.wheelbase * 0.44
# TODO: start from empirically derived lateral slip stiffness for the civic and scale by
# mass and CG position, so all cars will have approximately similar dyn behaviors
ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront,
tire_stiffness_factor=tire_stiffness_factor)
ret.enableBsm = 0x3F6 in fingerprint[0] and candidate in TSS2_CAR
# Detect smartDSU, which intercepts ACC_CMD from the DSU (or radar) allowing openpilot to send it
selfdrive/car/toyota/radar_interface.py
+38 -19
View File
@@ -1,4 +1,6 @@
#!/usr/bin/env python3
from collections import defaultdict
from opendbc.can.parser import CANParser
from cereal import car
from selfdrive.car.toyota.values import DBC, TSS2_CAR
@@ -15,7 +17,7 @@ def _create_radar_can_parser(car_fingerprint):
msg_a_n = len(RADAR_A_MSGS)
msg_b_n = len(RADAR_B_MSGS)
messages = list(zip(RADAR_A_MSGS + RADAR_B_MSGS, [20] * (msg_a_n + msg_b_n)))
messages = list(zip(RADAR_A_MSGS + RADAR_B_MSGS, [20] * (msg_a_n + msg_b_n), strict=True))
return CANParser(DBC[car_fingerprint]['radar'], messages, 1)
@@ -36,34 +38,47 @@ class RadarInterface(RadarInterfaceBase):
self.rcp = None if CP.radarUnavailable else _create_radar_can_parser(CP.carFingerprint)
self.trigger_msg = self.RADAR_B_MSGS[-1]
self.updated_messages = set()
self.updated_values = defaultdict(lambda: defaultdict(list))
def update(self, can_strings):
if self.rcp is None:
return super().update(None)
vls = self.rcp.update_strings(can_strings)
self.updated_messages.update(vls)
if self.trigger_msg not in self.updated_messages:
return None
rr = self._update(self.updated_messages)
self.updated_messages.clear()
addresses = self.rcp.update_strings(can_strings)
for addr in addresses:
vals_dict = self.rcp.vl_all[addr]
for sig_name, vals in vals_dict.items():
self.updated_values[addr][sig_name].extend(vals)
return rr
if self.trigger_msg not in self.updated_values:
return None
def _update(self, updated_messages):
radar_data = self._radar_msg_from_buffer(self.updated_values, self.rcp.can_valid)
self.updated_values.clear()
return radar_data
def _radar_msg_from_buffer(self, updated_values, can_valid):
ret = car.RadarData.new_message()
errors = []
if not self.rcp.can_valid:
if not can_valid:
errors.append("canError")
ret.errors = errors
for ii in sorted(updated_messages):
if ii in self.RADAR_A_MSGS:
cpt = self.rcp.vl[ii]
for ii in sorted(updated_values):
if ii not in self.RADAR_A_MSGS:
continue
radar_a_msgs = updated_values[ii]
radar_b_msgs = updated_values[ii+16]
n_vals_per_addr = len(list(radar_a_msgs.values())[0])
cpts = [
{k: v[i] for k, v in radar_a_msgs.items()}
for i in range(n_vals_per_addr)
]
for index, cpt in enumerate(cpts):
if cpt['LONG_DIST'] >= 255 or cpt['NEW_TRACK']:
self.valid_cnt[ii] = 0 # reset counter
if cpt['VALID'] and cpt['LONG_DIST'] < 255:
@@ -71,11 +86,15 @@ class RadarInterface(RadarInterfaceBase):
else:
self.valid_cnt[ii] = max(self.valid_cnt[ii] - 1, 0)
score = self.rcp.vl[ii+16]['SCORE']
# print ii, self.valid_cnt[ii], score, cpt['VALID'], cpt['LONG_DIST'], cpt['LAT_DIST']
n_b_scores = len(radar_b_msgs['SCORE'])
if n_b_scores > 0:
score_index = min(index, n_b_scores - 1)
score = radar_b_msgs['SCORE'][score_index]
else:
score = None
# radar point only valid if it's a valid measurement and score is above 50
if cpt['VALID'] or (score > 50 and cpt['LONG_DIST'] < 255 and self.valid_cnt[ii] > 0):
if cpt['VALID'] or (score and score > 50 and cpt['LONG_DIST'] < 255 and self.valid_cnt[ii] > 0):
if ii not in self.pts or cpt['NEW_TRACK']:
self.pts[ii] = car.RadarData.RadarPoint.new_message()
self.pts[ii].trackId = self.track_id
selfdrive/car/volkswagen/interface.py
+28 -28
View File
@@ -1,7 +1,7 @@
from cereal import car
from panda import Panda
from common.conversions import Conversions as CV
from selfdrive.car import STD_CARGO_KG, get_safety_config
from selfdrive.car import get_safety_config
from selfdrive.car.interfaces import CarInterfaceBase
from selfdrive.car.volkswagen.values import CAR, PQ_CARS, CANBUS, NetworkLocation, TransmissionType, GearShifter
@@ -101,32 +101,32 @@ class CarInterface(CarInterfaceBase):
# Per-chassis tuning values, override tuning defaults here if desired
if candidate == CAR.ARTEON_MK1:
ret.mass = 1733 + STD_CARGO_KG
ret.mass = 1733
ret.wheelbase = 2.84
elif candidate == CAR.ATLAS_MK1:
ret.mass = 2011 + STD_CARGO_KG
ret.mass = 2011
ret.wheelbase = 2.98
elif candidate == CAR.CRAFTER_MK2:
ret.mass = 2100 + STD_CARGO_KG
ret.mass = 2100
ret.wheelbase = 3.64 # SWB, LWB is 4.49, TBD how to detect difference
ret.minSteerSpeed = 50 * CV.KPH_TO_MS
elif candidate == CAR.GOLF_MK7:
ret.mass = 1397 + STD_CARGO_KG
ret.mass = 1397
ret.wheelbase = 2.62
elif candidate == CAR.JETTA_MK7:
ret.mass = 1328 + STD_CARGO_KG
ret.mass = 1328
ret.wheelbase = 2.71
elif candidate == CAR.PASSAT_MK8:
ret.mass = 1551 + STD_CARGO_KG
ret.mass = 1551
ret.wheelbase = 2.79
elif candidate == CAR.PASSAT_NMS:
ret.mass = 1503 + STD_CARGO_KG
ret.mass = 1503
ret.wheelbase = 2.80
ret.minEnableSpeed = 20 * CV.KPH_TO_MS # ACC "basic", no FtS
ret.minSteerSpeed = 50 * CV.KPH_TO_MS
@@ -134,86 +134,86 @@ class CarInterface(CarInterfaceBase):
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
elif candidate == CAR.POLO_MK6:
ret.mass = 1230 + STD_CARGO_KG
ret.mass = 1230
ret.wheelbase = 2.55
elif candidate == CAR.SHARAN_MK2:
ret.mass = 1639 + STD_CARGO_KG
ret.mass = 1639
ret.wheelbase = 2.92
ret.minSteerSpeed = 50 * CV.KPH_TO_MS
ret.steerActuatorDelay = 0.2
elif candidate == CAR.TAOS_MK1:
ret.mass = 1498 + STD_CARGO_KG
ret.mass = 1498
ret.wheelbase = 2.69
elif candidate == CAR.TCROSS_MK1:
ret.mass = 1150 + STD_CARGO_KG
ret.mass = 1150
ret.wheelbase = 2.60
elif candidate == CAR.TIGUAN_MK2:
ret.mass = 1715 + STD_CARGO_KG
ret.mass = 1715
ret.wheelbase = 2.74
elif candidate == CAR.TOURAN_MK2:
ret.mass = 1516 + STD_CARGO_KG
ret.mass = 1516
ret.wheelbase = 2.79
elif candidate == CAR.TRANSPORTER_T61:
ret.mass = 1926 + STD_CARGO_KG
ret.mass = 1926
ret.wheelbase = 3.00 # SWB, LWB is 3.40, TBD how to detect difference
ret.minSteerSpeed = 14.0
elif candidate == CAR.TROC_MK1:
ret.mass = 1413 + STD_CARGO_KG
ret.mass = 1413
ret.wheelbase = 2.63
elif candidate == CAR.AUDI_A3_MK3:
ret.mass = 1335 + STD_CARGO_KG
ret.mass = 1335
ret.wheelbase = 2.61
elif candidate == CAR.AUDI_Q2_MK1:
ret.mass = 1205 + STD_CARGO_KG
ret.mass = 1205
ret.wheelbase = 2.61
elif candidate == CAR.AUDI_Q3_MK2:
ret.mass = 1623 + STD_CARGO_KG
ret.mass = 1623
ret.wheelbase = 2.68
elif candidate == CAR.SEAT_ATECA_MK1:
ret.mass = 1900 + STD_CARGO_KG
ret.mass = 1900
ret.wheelbase = 2.64
elif candidate == CAR.SEAT_LEON_MK3:
ret.mass = 1227 + STD_CARGO_KG
ret.mass = 1227
ret.wheelbase = 2.64
elif candidate == CAR.SKODA_FABIA_MK4:
ret.mass = 1266 + STD_CARGO_KG
ret.mass = 1266
ret.wheelbase = 2.56
elif candidate == CAR.SKODA_KAMIQ_MK1:
ret.mass = 1265 + STD_CARGO_KG
ret.mass = 1265
ret.wheelbase = 2.66
elif candidate == CAR.SKODA_KAROQ_MK1:
ret.mass = 1278 + STD_CARGO_KG
ret.mass = 1278
ret.wheelbase = 2.66
elif candidate == CAR.SKODA_KODIAQ_MK1:
ret.mass = 1569 + STD_CARGO_KG
ret.mass = 1569
ret.wheelbase = 2.79
elif candidate == CAR.SKODA_OCTAVIA_MK3:
ret.mass = 1388 + STD_CARGO_KG
ret.mass = 1388
ret.wheelbase = 2.68
elif candidate == CAR.SKODA_SCALA_MK1:
ret.mass = 1192 + STD_CARGO_KG
ret.mass = 1192
ret.wheelbase = 2.65
elif candidate == CAR.SKODA_SUPERB_MK3:
ret.mass = 1505 + STD_CARGO_KG
ret.mass = 1505
ret.wheelbase = 2.84
else:
selfdrive/car/volkswagen/values.py
+6
View File
@@ -458,6 +458,7 @@ FW_VERSIONS = {
b'\xf1\x878V0906259H \xf1\x890002',
b'\xf1\x878V0906259J \xf1\x890003',
b'\xf1\x878V0906259K \xf1\x890001',
b'\xf1\x878V0906259K \xf1\x890003',
b'\xf1\x878V0906259P \xf1\x890001',
b'\xf1\x878V0906259Q \xf1\x890002',
b'\xf1\x878V0906264F \xf1\x890003',
@@ -486,6 +487,7 @@ FW_VERSIONS = {
b'\xf1\x870D9300012 \xf1\x894913',
b'\xf1\x870D9300012 \xf1\x894937',
b'\xf1\x870D9300012 \xf1\x895045',
b'\xf1\x870D9300012 \xf1\x895046',
b'\xf1\x870D9300014M \xf1\x895004',
b'\xf1\x870D9300014Q \xf1\x895006',
b'\xf1\x870D9300020J \xf1\x894902',
@@ -953,6 +955,7 @@ FW_VERSIONS = {
b'\xf1\x875G0906259D \xf1\x890002',
b'\xf1\x875G0906259L \xf1\x890002',
b'\xf1\x875G0906259Q \xf1\x890002',
b'\xf1\x878V0906259E \xf1\x890001',
b'\xf1\x878V0906259F \xf1\x890002',
b'\xf1\x878V0906259H \xf1\x890002',
b'\xf1\x878V0906259J \xf1\x890002',
@@ -967,6 +970,7 @@ FW_VERSIONS = {
b'\xf1\x870CW300048 \xf1\x895201',
b'\xf1\x870D9300012 \xf1\x894912',
b'\xf1\x870D9300012 \xf1\x894931',
b'\xf1\x870D9300012L \xf1\x894521',
b'\xf1\x870D9300012K \xf1\x894513',
b'\xf1\x870D9300013B \xf1\x894902',
b'\xf1\x870D9300013B \xf1\x894931',
@@ -997,6 +1001,7 @@ FW_VERSIONS = {
b'\xf1\x875Q0959655N \xf1\x890361\xf1\x82\0211212001112111104110411111521159114',
],
(Ecu.eps, 0x712, None): [
b'\xf1\x873Q0909144F \xf1\x895043\xf1\x82\x0561G01A13A0',
b'\xf1\x873Q0909144H \xf1\x895061\xf1\x82\00566G0HA14A1',
b'\xf1\x873Q0909144J \xf1\x895063\xf1\x82\x0566G0HA14A1',
b'\xf1\x873Q0909144K \xf1\x895072\xf1\x82\x0571G01A16A1',
@@ -1012,6 +1017,7 @@ FW_VERSIONS = {
b'\xf1\x875Q0909144T \xf1\x891072\xf1\x82\00521G00807A1',
],
(Ecu.fwdRadar, 0x757, None): [
b'\xf1\x875Q0907567M \xf1\x890398\xf1\x82\x0101',
b'\xf1\x875Q0907567N \xf1\x890400\xf1\x82\x0101',
b'\xf1\x875Q0907572F \xf1\x890400\xf1\x82\x0101',
b'\xf1\x875Q0907572D \xf1\x890304\xf1\x82\x0101',
selfdrive/controls/lib/events.py
+2 -2
View File
@@ -238,7 +238,7 @@ def below_steer_speed_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.S
f"Steer Unavailable Below {get_display_speed(CP.minSteerSpeed, metric)}",
"",
AlertStatus.userPrompt, AlertSize.small,
Priority.MID, VisualAlert.steerRequired, AudibleAlert.prompt, 0.4)
Priority.LOW, VisualAlert.steerRequired, AudibleAlert.prompt, 0.4)
def calibration_incomplete_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert:
@@ -505,7 +505,7 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = {
"Press Resume to Exit Standstill",
"",
AlertStatus.userPrompt, AlertSize.small,
Priority.LOW, VisualAlert.none, AudibleAlert.none, .2),
Priority.MID, VisualAlert.none, AudibleAlert.none, .2),
},
EventName.belowSteerSpeed: {
selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py
+6 -2
View File
@@ -129,11 +129,15 @@ def gen_lat_ocp():
class LateralMpc():
def __init__(self, x0=np.zeros(X_DIM)):
def __init__(self, x0=None):
if x0 is None:
x0 = np.zeros(X_DIM)
self.solver = AcadosOcpSolverCython(MODEL_NAME, ACADOS_SOLVER_TYPE, N)
self.reset(x0)
def reset(self, x0=np.zeros(X_DIM)):
def reset(self, x0=None):
if x0 is None:
x0 = np.zeros(X_DIM)
self.x_sol = np.zeros((N+1, X_DIM))
self.u_sol = np.zeros((N, 1))
self.yref = np.zeros((N+1, COST_DIM))
selfdrive/controls/lib/lateral_planner.py
+4 -2
View File
@@ -50,7 +50,9 @@ class LateralPlanner:
self.lat_mpc = LateralMpc()
self.reset_mpc(np.zeros(4))
def reset_mpc(self, x0=np.zeros(4)):
def reset_mpc(self, x0=None):
if x0 is None:
x0 = np.zeros(4)
self.x0 = x0
self.lat_mpc.reset(x0=self.x0)
@@ -130,7 +132,7 @@ class LateralPlanner:
lateralPlan.psis = self.lat_mpc.x_sol[0:CONTROL_N, 2].tolist()
lateralPlan.curvatures = (self.lat_mpc.x_sol[0:CONTROL_N, 3]/self.v_ego).tolist()
lateralPlan.curvatureRates = [float(x/self.v_ego) for x in self.lat_mpc.u_sol[0:CONTROL_N - 1]] + [0.0]
lateralPlan.curvatureRates = [float(x.item() / self.v_ego) for x in self.lat_mpc.u_sol[0:CONTROL_N - 1]] + [0.0]
lateralPlan.mpcSolutionValid = bool(plan_solution_valid)
lateralPlan.solverExecutionTime = self.lat_mpc.solve_time
selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py
+3 -1
View File
@@ -83,7 +83,9 @@ def get_stopped_equivalence_factor(v_lead):
def get_safe_obstacle_distance(v_ego, t_follow):
return (v_ego**2) / (2 * COMFORT_BRAKE) + t_follow * v_ego + STOP_DISTANCE
def desired_follow_distance(v_ego, v_lead, t_follow=get_T_FOLLOW()):
def desired_follow_distance(v_ego, v_lead, t_follow=None):
if t_follow is None:
t_follow = get_T_FOLLOW()
return get_safe_obstacle_distance(v_ego, t_follow) - get_stopped_equivalence_factor(v_lead)
selfdrive/controls/radard.py
+57 -42
View File
@@ -8,7 +8,7 @@ import capnp
from cereal import messaging, log, car
from common.numpy_fast import interp
from common.params import Params
from common.realtime import Ratekeeper, Priority, config_realtime_process
from common.realtime import Ratekeeper, Priority, config_realtime_process, DT_MDL
from system.swaglog import cloudlog
from common.kalman.simple_kalman import KF1D
@@ -50,7 +50,8 @@ class KalmanParams:
class Track:
def __init__(self, v_lead: float, kalman_params: KalmanParams):
def __init__(self, identifier: int, v_lead: float, kalman_params: KalmanParams):
self.identifier = identifier
self.cnt = 0
self.aLeadTau = _LEAD_ACCEL_TAU
self.K_A = kalman_params.A
@@ -63,9 +64,13 @@ class Track:
self.dRel = d_rel # LONG_DIST
self.yRel = y_rel # -LAT_DIST
self.vRel = v_rel # REL_SPEED
self.vLead = v_lead
self.measured = measured # measured or estimate
self.update_vlead(v_lead)
def update_vlead(self, v_lead: float):
self.vLead = v_lead
# computed velocity and accelerations
if self.cnt > 0:
self.kf.update(self.vLead)
@@ -98,11 +103,12 @@ class Track:
"vLead": float(self.vLead),
"vLeadK": float(self.vLeadK),
"aLeadK": float(self.aLeadK),
"aLeadTau": float(self.aLeadTau),
"status": True,
"fcw": self.is_potential_fcw(model_prob),
"modelProb": model_prob,
"radar": True,
"aLeadTau": float(self.aLeadTau)
"radarTrackId": self.identifier,
}
def potential_low_speed_lead(self, v_ego: float):
@@ -158,15 +164,17 @@ def get_RadarState_from_vision(lead_msg: capnp._DynamicStructReader, v_ego: floa
"aLeadTau": 0.3,
"fcw": False,
"modelProb": float(lead_msg.prob),
"status": True,
"radar": False,
"status": True
"radarTrackId": -1,
}
def get_lead(v_ego: float, ready: bool, tracks: Dict[int, Track], lead_msg: capnp._DynamicStructReader,
model_v_ego: float, low_speed_override: bool = True) -> Dict[str, Any]:
# Determine leads, this is where the essential logic happens
if len(tracks) > 0 and ready and lead_msg.prob > .5:
lead_msg_empty = any(len(c) == 0 for c in [lead_msg.x, lead_msg.y, lead_msg.v])
if len(tracks) > 0 and ready and not lead_msg_empty and lead_msg.prob > .5:
track = match_vision_to_track(v_ego, lead_msg, tracks)
else:
track = None
@@ -174,7 +182,7 @@ def get_lead(v_ego: float, ready: bool, tracks: Dict[int, Track], lead_msg: capn
lead_dict = {'status': False}
if track is not None:
lead_dict = track.get_RadarState(lead_msg.prob)
elif (track is None) and ready and (lead_msg.prob > .5):
elif track is None and ready and not lead_msg_empty and lead_msg.prob > .5:
lead_dict = get_RadarState_from_vision(lead_msg, v_ego, model_v_ego)
if low_speed_override:
@@ -204,14 +212,14 @@ class RadarD:
self.ready = False
def update(self, sm: messaging.SubMaster, rr: Optional[car.RadarData]):
def update(self, sm: messaging.SubMaster, radar_data: Optional[car.RadarData]):
self.current_time = 1e-9*max(sm.logMonoTime.values())
radar_points = []
radar_errors = []
if rr is not None:
radar_points = rr.points
radar_errors = rr.errors
if radar_data is not None:
radar_points = radar_data.points
radar_errors = radar_data.errors
if sm.updated['carState']:
self.v_ego = sm['carState'].vEgo
@@ -219,26 +227,32 @@ class RadarD:
if sm.updated['modelV2']:
self.ready = True
ar_pts = {}
for pt in radar_points:
ar_pts[pt.trackId] = [pt.dRel, pt.yRel, pt.vRel, pt.measured]
if radar_data is not None:
ar_pts = {}
for pt in radar_points:
ar_pts[pt.trackId] = [pt.dRel, pt.yRel, pt.vRel, pt.measured]
# *** remove missing points from meta data ***
for ids in list(self.tracks.keys()):
if ids not in ar_pts:
self.tracks.pop(ids, None)
# *** remove missing points from meta data ***
for ids in list(self.tracks.keys()):
if ids not in ar_pts:
self.tracks.pop(ids, None)
# *** compute the tracks ***
for ids in ar_pts:
rpt = ar_pts[ids]
# *** compute the tracks ***
for ids in ar_pts:
rpt = ar_pts[ids]
# align v_ego by a fixed time to align it with the radar measurement
v_lead = rpt[2] + self.v_ego_hist[0]
# align v_ego by a fixed time to align it with the radar measurement
v_lead = rpt[2] + self.v_ego_hist[0]
# create the track if it doesn't exist or it's a new track
if ids not in self.tracks:
self.tracks[ids] = Track(v_lead, self.kalman_params)
self.tracks[ids].update(rpt[0], rpt[1], rpt[2], v_lead, rpt[3])
# create the track if it doesn't exist or it's a new track
if ids not in self.tracks:
self.tracks[ids] = Track(ids, v_lead, self.kalman_params)
self.tracks[ids].update(rpt[0], rpt[1], rpt[2], v_lead, rpt[3])
else:
# *** no radar points, keep existing tracks, update v_lead
for track in self.tracks.values():
v_lead = track.vRel + self.v_ego_hist[0]
track.update_vlead(v_lead)
# *** publish radarState ***
self.radar_state_valid = sm.all_checks() and len(radar_errors) == 0
@@ -291,32 +305,33 @@ def radard_thread(sm: Optional[messaging.SubMaster] = None, pm: Optional[messagi
cloudlog.info("radard is importing %s", CP.carName)
RadarInterface = importlib.import_module(f'selfdrive.car.{CP.carName}.radar_interface').RadarInterface
# *** setup messaging
# setup messaging
if can_sock is None:
can_sock = messaging.sub_sock('can')
if sm is None:
sm = messaging.SubMaster(['modelV2', 'carState'], ignore_avg_freq=['modelV2', 'carState']) # Can't check average frequency, since radar determines timing
sm = messaging.SubMaster(['modelV2', 'carState'], poll=["modelV2"])
if pm is None:
pm = messaging.PubMaster(['radarState', 'liveTracks'])
RI = RadarInterface(CP)
interface = RadarInterface(CP)
rk = Ratekeeper(1.0 / CP.radarTimeStep, print_delay_threshold=None)
RD = RadarD(CP.radarTimeStep, RI.delay)
rk = Ratekeeper(1 / DT_MDL, print_delay_threshold=None)
radar = RadarD(DT_MDL, interface.delay)
while 1:
can_strings = messaging.drain_sock_raw(can_sock, wait_for_one=True)
rr = RI.update(can_strings)
while True:
sm.update()
if rr is None:
continue
if sm.updated['modelV2']:
can_strings = messaging.drain_sock_raw(can_sock)
if len(can_strings) == 0:
radar_data = None
else:
radar_data = interface.update(can_strings)
sm.update(0)
radar.update(sm, radar_data)
radar.publish(pm, -rk.remaining*1000.0)
RD.update(sm, rr)
RD.publish(pm, -rk.remaining*1000.0)
rk.monitor_time()
rk.monitor_time()
def main(sm: Optional[messaging.SubMaster] = None, pm: Optional[messaging.PubMaster] = None, can_sock: messaging.SubSocket = None):
selfdrive/controls/tests/test_clustering.py
-140
View File
@@ -1,140 +0,0 @@
# pylint: skip-file
import time
import unittest
import numpy as np
from fastcluster import linkage_vector
from scipy.cluster import _hierarchy
from scipy.spatial.distance import pdist
from third_party.cluster.fastcluster_py import hclust, ffi
from third_party.cluster.fastcluster_py import cluster_points_centroid
def fcluster(Z, t, criterion='inconsistent', depth=2, R=None, monocrit=None):
# supersimplified function to get fast clustering. Got it from scipy
Z = np.asarray(Z, order='c')
n = Z.shape[0] + 1
T = np.zeros((n,), dtype='i')
_hierarchy.cluster_dist(Z, T, float(t), int(n))
return T
TRACK_PTS = np.array([[59.26000137, -9.35999966, -5.42500019],
[91.61999817, -0.31999999, -2.75],
[31.38000031, 0.40000001, -0.2],
[89.57999725, -8.07999992, -18.04999924],
[53.42000122, 0.63999999, -0.175],
[31.38000031, 0.47999999, -0.2],
[36.33999939, 0.16, -0.2],
[53.33999939, 0.95999998, -0.175],
[59.26000137, -9.76000023, -5.44999981],
[33.93999977, 0.40000001, -0.22499999],
[106.74000092, -5.76000023, -18.04999924]])
CORRECT_LINK = np.array([[2., 5., 0.07999998, 2.],
[4., 7., 0.32984889, 2.],
[0., 8., 0.40078104, 2.],
[6., 9., 2.41209933, 2.],
[11., 14., 3.76342275, 4.],
[12., 13., 13.02297651, 4.],
[1., 3., 17.27626057, 2.],
[10., 17., 17.92918845, 3.],
[15., 16., 23.68525366, 8.],
[18., 19., 52.52351319, 11.]])
CORRECT_LABELS = np.array([7, 1, 4, 2, 6, 4, 5, 6, 7, 5, 3], dtype=np.int32)
def plot_cluster(pts, idx_old, idx_new):
import matplotlib.pyplot as plt
m = 'Set1'
plt.figure()
plt.subplot(1, 2, 1)
plt.scatter(pts[:, 0], pts[:, 1], c=idx_old, cmap=m)
plt.title("Old")
plt.colorbar()
plt.subplot(1, 2, 2)
plt.scatter(pts[:, 0], pts[:, 1], c=idx_new, cmap=m)
plt.title("New")
plt.colorbar()
plt.show()
def same_clusters(correct, other):
correct = np.asarray(correct)
other = np.asarray(other)
if len(correct) != len(other):
return False
for i in range(len(correct)):
c = np.where(correct == correct[i])
o = np.where(other == other[i])
if not np.array_equal(c, o):
return False
return True
class TestClustering(unittest.TestCase):
def test_scipy_clustering(self):
old_link = linkage_vector(TRACK_PTS, method='centroid')
old_cluster_idxs = fcluster(old_link, 2.5, criterion='distance')
np.testing.assert_allclose(old_link, CORRECT_LINK)
np.testing.assert_allclose(old_cluster_idxs, CORRECT_LABELS)
def test_pdist(self):
pts = np.ascontiguousarray(TRACK_PTS, dtype=np.float64)
pts_ptr = ffi.cast("double *", pts.ctypes.data)
n, m = pts.shape
out = np.zeros((n * (n - 1) // 2, ), dtype=np.float64)
out_ptr = ffi.cast("double *", out.ctypes.data)
hclust.hclust_pdist(n, m, pts_ptr, out_ptr)
np.testing.assert_allclose(out, np.power(pdist(TRACK_PTS), 2))
def test_cpp_clustering(self):
pts = np.ascontiguousarray(TRACK_PTS, dtype=np.float64)
pts_ptr = ffi.cast("double *", pts.ctypes.data)
n, m = pts.shape
labels = np.zeros((n, ), dtype=np.int32)
labels_ptr = ffi.cast("int *", labels.ctypes.data)
hclust.cluster_points_centroid(n, m, pts_ptr, 2.5**2, labels_ptr)
self.assertTrue(same_clusters(CORRECT_LABELS, labels))
def test_cpp_wrapper_clustering(self):
labels = cluster_points_centroid(TRACK_PTS, 2.5)
self.assertTrue(same_clusters(CORRECT_LABELS, labels))
def test_random_cluster(self):
np.random.seed(1337)
N = 1000
t_old = 0.
t_new = 0.
for _ in range(N):
n = int(np.random.uniform(2, 32))
x = np.random.uniform(-10, 50, (n, 1))
y = np.random.uniform(-5, 5, (n, 1))
vrel = np.random.uniform(-5, 5, (n, 1))
pts = np.hstack([x, y, vrel])
t = time.time()
old_link = linkage_vector(pts, method='centroid')
old_cluster_idx = fcluster(old_link, 2.5, criterion='distance')
t_old += time.time() - t
t = time.time()
cluster_idx = cluster_points_centroid(pts, 2.5)
t_new += time.time() - t
self.assertTrue(same_clusters(old_cluster_idx, cluster_idx))
if __name__ == "__main__":
unittest.main()
selfdrive/debug/check_can_parser_performance.py
Executable
+37
View File
@@ -0,0 +1,37 @@
#!/usr/bin/env python3
import numpy as np
import time
from tqdm import tqdm
from cereal import car
from selfdrive.car.tests.routes import CarTestRoute
from selfdrive.car.tests.test_models import TestCarModelBase
from tools.plotjuggler.juggle import DEMO_ROUTE
N_RUNS = 10
class CarModelTestCase(TestCarModelBase):
test_route = CarTestRoute(DEMO_ROUTE, None)
ci = False
if __name__ == '__main__':
# Get CAN messages and parsers
tm = CarModelTestCase()
tm.setUpClass()
tm.setUp()
CC = car.CarControl.new_message()
ets = []
for _ in tqdm(range(N_RUNS)):
start_t = time.process_time_ns()
for msg in tm.can_msgs:
for cp in tm.CI.can_parsers:
if cp is not None:
cp.update_strings((msg.as_builder().to_bytes(),))
ets.append((time.process_time_ns() - start_t) * 1e-6)
print(f'{len(tm.can_msgs)} CAN packets, {N_RUNS} runs')
print(f'{np.mean(ets):.2f} mean ms, {max(ets):.2f} max ms, {min(ets):.2f} min ms, {np.std(ets):.2f} std ms')
print(f'{np.mean(ets) / len(tm.can_msgs):.4f} mean ms / CAN packet')
selfdrive/debug/filter_log_message.py
+1 -1
View File
@@ -59,7 +59,7 @@ if __name__ == "__main__":
logs = [args.route[0]]
else:
r = Route(args.route[0])
logs = [q_log if r_log is None else r_log for (q_log, r_log) in zip(r.qlog_paths(), r.log_paths())]
logs = [q_log if r_log is None else r_log for (q_log, r_log) in zip(r.qlog_paths(), r.log_paths(), strict=True)]
if len(args.route) == 2 and logs:
n = int(args.route[1])
selfdrive/debug/internal/fuzz_fw_fingerprint.py
+1 -1
View File
@@ -27,7 +27,7 @@ if __name__ == "__main__":
for _ in tqdm(range(1000)):
for candidate, fws in FWS.items():
fw_dict = {}
for (tp, addr, subaddr), fw_list in fws.items():
for (_, addr, subaddr), fw_list in fws.items():
fw_dict[(addr, subaddr)] = [random.choice(fw_list)]
matches = match_fw_to_car_fuzzy(fw_dict, log=False, exclude=candidate)
selfdrive/debug/internal/qlog_size.py
+1 -1
View File
@@ -27,7 +27,7 @@ def make_pie(msgs, typ):
sizes_large = [(k, sz) for (k, sz) in sizes if sz >= total * MIN_SIZE / 100]
sizes_large += [('other', sum(sz for (_, sz) in sizes if sz < total * MIN_SIZE / 100))]
labels, sizes = zip(*sizes_large)
labels, sizes = zip(*sizes_large, strict=True)
plt.figure()
plt.title(f"{typ}")
selfdrive/locationd/models/loc_kf.py
+6 -6
View File
@@ -191,9 +191,9 @@ class LocKalman():
# Observation matrix modifier
H_mod_sym = sp.Matrix(np.zeros((dim_state, dim_state_err)))
for p_idx, p_err_idx in zip(p_idxs, p_err_idxs):
for p_idx, p_err_idx in zip(p_idxs, p_err_idxs, strict=True):
H_mod_sym[p_idx[0]:p_idx[1], p_err_idx[0]:p_err_idx[1]] = np.eye(p_idx[1] - p_idx[0])
for q_idx, q_err_idx in zip(q_idxs, q_err_idxs):
for q_idx, q_err_idx in zip(q_idxs, q_err_idxs, strict=True):
H_mod_sym[q_idx[0]:q_idx[1], q_err_idx[0]:q_err_idx[1]] = 0.5 * quat_matrix_r(state[q_idx[0]:q_idx[1]])[:, 1:]
# these error functions are defined so that say there
@@ -205,17 +205,17 @@ class LocKalman():
delta_x = sp.MatrixSymbol('delta_x', dim_state_err, 1)
err_function_sym = sp.Matrix(np.zeros((dim_state, 1)))
for q_idx, q_err_idx in zip(q_idxs, q_err_idxs):
for q_idx, q_err_idx in zip(q_idxs, q_err_idxs, strict=True):
delta_quat = sp.Matrix(np.ones(4))
delta_quat[1:, :] = sp.Matrix(0.5 * delta_x[q_err_idx[0]: q_err_idx[1], :])
err_function_sym[q_idx[0]:q_idx[1], 0] = quat_matrix_r(nom_x[q_idx[0]:q_idx[1], 0]) * delta_quat
for p_idx, p_err_idx in zip(p_idxs, p_err_idxs):
for p_idx, p_err_idx in zip(p_idxs, p_err_idxs, strict=True):
err_function_sym[p_idx[0]:p_idx[1], :] = sp.Matrix(nom_x[p_idx[0]:p_idx[1], :] + delta_x[p_err_idx[0]:p_err_idx[1], :])
inv_err_function_sym = sp.Matrix(np.zeros((dim_state_err, 1)))
for p_idx, p_err_idx in zip(p_idxs, p_err_idxs):
for p_idx, p_err_idx in zip(p_idxs, p_err_idxs, strict=True):
inv_err_function_sym[p_err_idx[0]:p_err_idx[1], 0] = sp.Matrix(-nom_x[p_idx[0]:p_idx[1], 0] + true_x[p_idx[0]:p_idx[1], 0])
for q_idx, q_err_idx in zip(q_idxs, q_err_idxs):
for q_idx, q_err_idx in zip(q_idxs, q_err_idxs, strict=True):
delta_quat = quat_matrix_r(nom_x[q_idx[0]:q_idx[1], 0]).T * true_x[q_idx[0]:q_idx[1], 0]
inv_err_function_sym[q_err_idx[0]:q_err_idx[1], 0] = sp.Matrix(2 * delta_quat[1:])
selfdrive/locationd/paramsd.py
+13 -13
View File
@@ -85,8 +85,8 @@ class ParamsLearner:
# We observe the current stiffness and steer ratio (with a high observation noise) to bound
# the respective estimate STD. Otherwise the STDs keep increasing, causing rapid changes in the
# states in longer routes (especially straight stretches).
stiffness = float(self.kf.x[States.STIFFNESS])
steer_ratio = float(self.kf.x[States.STEER_RATIO])
stiffness = float(self.kf.x[States.STIFFNESS].item())
steer_ratio = float(self.kf.x[States.STEER_RATIO].item())
self.kf.predict_and_observe(t, ObservationKind.STIFFNESS, np.array([[stiffness]]))
self.kf.predict_and_observe(t, ObservationKind.STEER_RATIO, np.array([[steer_ratio]]))
@@ -198,14 +198,14 @@ def main(sm=None, pm=None):
learner = ParamsLearner(CP, CP.steerRatio, 1.0, 0.0)
x = learner.kf.x
angle_offset_average = clip(math.degrees(x[States.ANGLE_OFFSET]),
angle_offset_average = clip(math.degrees(x[States.ANGLE_OFFSET].item()),
angle_offset_average - MAX_ANGLE_OFFSET_DELTA, angle_offset_average + MAX_ANGLE_OFFSET_DELTA)
angle_offset = clip(math.degrees(x[States.ANGLE_OFFSET] + x[States.ANGLE_OFFSET_FAST]),
angle_offset = clip(math.degrees(x[States.ANGLE_OFFSET].item() + x[States.ANGLE_OFFSET_FAST].item()),
angle_offset - MAX_ANGLE_OFFSET_DELTA, angle_offset + MAX_ANGLE_OFFSET_DELTA)
roll = clip(float(x[States.ROAD_ROLL]), roll - ROLL_MAX_DELTA, roll + ROLL_MAX_DELTA)
roll_std = float(P[States.ROAD_ROLL])
roll = clip(float(x[States.ROAD_ROLL].item()), roll - ROLL_MAX_DELTA, roll + ROLL_MAX_DELTA)
roll_std = float(P[States.ROAD_ROLL].item())
# Account for the opposite signs of the yaw rates
sensors_valid = bool(abs(learner.speed * (x[States.YAW_RATE] + learner.yaw_rate)) < LATERAL_ACC_SENSOR_THRESHOLD)
sensors_valid = bool(abs(learner.speed * (x[States.YAW_RATE].item() + learner.yaw_rate)) < LATERAL_ACC_SENSOR_THRESHOLD)
avg_offset_valid = check_valid_with_hysteresis(avg_offset_valid, angle_offset_average, OFFSET_MAX, OFFSET_LOWERED_MAX)
total_offset_valid = check_valid_with_hysteresis(total_offset_valid, angle_offset, OFFSET_MAX, OFFSET_LOWERED_MAX)
roll_valid = check_valid_with_hysteresis(roll_valid, roll, ROLL_MAX, ROLL_LOWERED_MAX)
@@ -215,8 +215,8 @@ def main(sm=None, pm=None):
liveParameters = msg.liveParameters
liveParameters.posenetValid = True
liveParameters.sensorValid = sensors_valid
liveParameters.steerRatio = float(x[States.STEER_RATIO])
liveParameters.stiffnessFactor = float(x[States.STIFFNESS])
liveParameters.steerRatio = float(x[States.STEER_RATIO].item())
liveParameters.stiffnessFactor = float(x[States.STIFFNESS].item())
liveParameters.roll = roll
liveParameters.angleOffsetAverageDeg = angle_offset_average
liveParameters.angleOffsetDeg = angle_offset
@@ -228,10 +228,10 @@ def main(sm=None, pm=None):
0.2 <= liveParameters.stiffnessFactor <= 5.0,
min_sr <= liveParameters.steerRatio <= max_sr,
))
liveParameters.steerRatioStd = float(P[States.STEER_RATIO])
liveParameters.stiffnessFactorStd = float(P[States.STIFFNESS])
liveParameters.angleOffsetAverageStd = float(P[States.ANGLE_OFFSET])
liveParameters.angleOffsetFastStd = float(P[States.ANGLE_OFFSET_FAST])
liveParameters.steerRatioStd = float(P[States.STEER_RATIO].item())
liveParameters.stiffnessFactorStd = float(P[States.STIFFNESS].item())
liveParameters.angleOffsetAverageStd = float(P[States.ANGLE_OFFSET].item())
liveParameters.angleOffsetFastStd = float(P[States.ANGLE_OFFSET_FAST].item())
if DEBUG:
liveParameters.filterState = log.LiveLocationKalman.Measurement.new_message()
liveParameters.filterState.value = x.tolist()
selfdrive/locationd/test/test_laikad.py
+3 -3
View File
@@ -173,7 +173,7 @@ class TestLaikad(unittest.TestCase):
self.assertTrue(kf_valid)
def test_laika_online_nav_only(self):
for use_qcom, logs in zip([True, False], [self.logs_qcom, self.logs]):
for use_qcom, logs in zip([True, False], [self.logs_qcom, self.logs], strict=True):
laikad = Laikad(auto_update=True, valid_ephem_types=EphemerisType.NAV, use_qcom=use_qcom)
# Disable fetch_orbits to test NAV only
correct_msgs = verify_messages(logs, laikad)
@@ -215,7 +215,7 @@ class TestLaikad(unittest.TestCase):
def test_get_navs_in_process(self):
for auto_fetch_navs in [True, False]:
for use_qcom, logs in zip([True, False], [self.logs_qcom, self.logs]):
for use_qcom, logs in zip([True, False], [self.logs_qcom, self.logs], strict=True):
laikad = Laikad(auto_update=False, use_qcom=use_qcom, auto_fetch_navs=auto_fetch_navs)
has_navs = False
has_fix = False
@@ -248,7 +248,7 @@ class TestLaikad(unittest.TestCase):
def test_cache(self):
use_qcom = True
for use_qcom, logs in zip([True, False], [self.logs_qcom, self.logs]):
for use_qcom, logs in zip([True, False], [self.logs_qcom, self.logs], strict=True):
Params().remove(EPHEMERIS_CACHE)
laikad = Laikad(auto_update=True, save_ephemeris=True, use_qcom=use_qcom)
def wait_for_cache():
selfdrive/locationd/torqued.py
+3 -2
View File
@@ -63,7 +63,7 @@ class PointBuckets:
def __init__(self, x_bounds, min_points, min_points_total):
self.x_bounds = x_bounds
self.buckets = {bounds: NPQueue(maxlen=POINTS_PER_BUCKET, rowsize=3) for bounds in x_bounds}
self.buckets_min_points = dict(zip(x_bounds, min_points))
self.buckets_min_points = dict(zip(x_bounds, min_points, strict=True))
self.min_points_total = min_points_total
def bucket_lengths(self):
@@ -73,7 +73,8 @@ class PointBuckets:
return sum(self.bucket_lengths())
def is_valid(self):
return all(len(v) >= min_pts for v, min_pts in zip(self.buckets.values(), self.buckets_min_points.values())) and (self.__len__() >= self.min_points_total)
return all(len(v) >= min_pts for v, min_pts in zip(self.buckets.values(), self.buckets_min_points.values(), strict=True)) \
and (self.__len__() >= self.min_points_total)
def add_point(self, x, y):
for bound_min, bound_max in self.x_bounds:
selfdrive/manager/build.py
+1 -1
View File
@@ -15,7 +15,7 @@ from system.version import is_dirty
MAX_CACHE_SIZE = 4e9 if "CI" in os.environ else 2e9
CACHE_DIR = Path("/data/scons_cache" if AGNOS else "/tmp/scons_cache")
TOTAL_SCONS_NODES = 2460
TOTAL_SCONS_NODES = 2560
MAX_BUILD_PROGRESS = 100
PREBUILT = os.path.exists(os.path.join(BASEDIR, 'prebuilt'))
selfdrive/manager/process.py
+1 -1
View File
@@ -39,7 +39,7 @@ def launcher(proc: str, name: str) -> None:
sentry.set_tag("daemon", name)
# exec the process
getattr(mod, 'main')()
mod.main()
except KeyboardInterrupt:
cloudlog.warning(f"child {proc} got SIGINT")
except Exception:
selfdrive/manager/process_config.py
+5 -1
View File
@@ -13,6 +13,9 @@ def driverview(started: bool, params: Params, CP: car.CarParams) -> bool:
def notcar(started: bool, params: Params, CP: car.CarParams) -> bool:
return CP.notCar # type: ignore
def iscar(started: bool, params: Params, CP: car.CarParams) -> bool:
return not CP.notCar
def logging(started, params, CP: car.CarParams) -> bool:
run = (not CP.notCar) or not params.get_bool("DisableLogging")
return started and run
@@ -35,12 +38,13 @@ procs = [
NativeProcess("logcatd", "system/logcatd", ["./logcatd"]),
NativeProcess("proclogd", "system/proclogd", ["./proclogd"]),
PythonProcess("logmessaged", "system.logmessaged", offroad=True),
PythonProcess("micd", "system.micd"),
PythonProcess("micd", "system.micd", callback=iscar),
PythonProcess("timezoned", "system.timezoned", enabled=not PC, offroad=True),
DaemonProcess("manage_athenad", "selfdrive.athena.manage_athenad", "AthenadPid"),
NativeProcess("dmonitoringmodeld", "selfdrive/modeld", ["./dmonitoringmodeld"], enabled=(not PC or WEBCAM), callback=driverview),
NativeProcess("encoderd", "system/loggerd", ["./encoderd"]),
NativeProcess("stream_encoderd", "system/loggerd", ["./encoderd", "--stream"], onroad=False, callback=notcar),
NativeProcess("loggerd", "system/loggerd", ["./loggerd"], onroad=False, callback=logging),
NativeProcess("modeld", "selfdrive/modeld", ["./modeld"]),
NativeProcess("mapsd", "selfdrive/navd", ["./mapsd"]),
selfdrive/modeld/runners/onnx_runner.py
+3 -3
View File
@@ -36,16 +36,16 @@ def run_loop(m, tf8_input=False):
# run once to initialize CUDA provider
if "CUDAExecutionProvider" in m.get_providers():
m.run(None, dict(zip(keys, [np.zeros(shp, dtype=itp) for shp, itp in zip(ishapes, itypes)])))
m.run(None, dict(zip(keys, [np.zeros(shp, dtype=itp) for shp, itp in zip(ishapes, itypes, strict=True)], strict=True)))
print("ready to run onnx model", keys, ishapes, file=sys.stderr)
while 1:
inputs = []
for k, shp, itp in zip(keys, ishapes, itypes):
for k, shp, itp in zip(keys, ishapes, itypes, strict=True):
ts = np.product(shp)
#print("reshaping %s with offset %d" % (str(shp), offset), file=sys.stderr)
inputs.append(read(ts, (k=='input_img' and tf8_input)).reshape(shp).astype(itp))
ret = m.run(None, dict(zip(keys, inputs)))
ret = m.run(None, dict(zip(keys, inputs, strict=True)))
#print(ret, file=sys.stderr)
for r in ret:
write(r.astype(np.float32))
selfdrive/modeld/tests/timing/benchmark.py
+1 -1
View File
@@ -35,6 +35,6 @@ if __name__ == "__main__":
print("\n\n")
print(f"ran modeld {N} times for {TIME}s each")
for n, t in enumerate(execution_times):
for _, t in enumerate(execution_times):
print(f"\tavg: {sum(t)/len(t):0.2f}ms, min: {min(t):0.2f}ms, max: {max(t):0.2f}ms")
print("\n\n")
selfdrive/monitoring/driver_monitor.py
+3 -1
View File
@@ -119,7 +119,9 @@ class DriverBlink():
self.right_blink = 0.
class DriverStatus():
def __init__(self, rhd_saved=False, settings=DRIVER_MONITOR_SETTINGS()):
def __init__(self, rhd_saved=False, settings=None):
if settings is None:
settings = DRIVER_MONITOR_SETTINGS()
# init policy settings
self.settings = settings
selfdrive/navd/helpers.py
+6 -1
View File
@@ -8,6 +8,9 @@ from common.conversions import Conversions
from common.numpy_fast import clip
from common.params import Params
DIRECTIONS = ('left', 'right', 'straight')
MODIFIABLE_DIRECTIONS = ('left', 'right')
EARTH_MEAN_RADIUS = 6371007.2
SPEED_CONVERSIONS = {
'km/h': Conversions.KPH_TO_MS,
@@ -119,8 +122,10 @@ def coordinate_from_param(param: str, params: Optional[Params] = None) -> Option
def string_to_direction(direction: str) -> str:
for d in ['left', 'right', 'straight']:
for d in DIRECTIONS:
if d in direction:
if 'slight' in direction and d in MODIFIABLE_DIRECTIONS:
return 'slight' + d.capitalize()
return d
return 'none'
selfdrive/statsd.py
+67 -55
View File
@@ -23,6 +23,8 @@ class METRIC_TYPE:
class StatLog:
def __init__(self):
self.pid = None
self.zctx = None
self.sock = None
def connect(self) -> None:
self.zctx = zmq.Context()
@@ -31,6 +33,12 @@ class StatLog:
self.sock.connect(STATS_SOCKET)
self.pid = os.getpid()
def __del__(self):
if self.sock is not None:
self.sock.close()
if self.zctx is not None:
self.zctx.term()
def _send(self, metric: str) -> None:
if os.getpid() != self.pid:
self.connect()
@@ -68,7 +76,7 @@ def main() -> NoReturn:
return res
# open statistics socket
ctx = zmq.Context().instance()
ctx = zmq.Context.instance()
sock = ctx.socket(zmq.PULL)
sock.bind(STATS_SOCKET)
@@ -92,70 +100,74 @@ def main() -> NoReturn:
last_flush_time = time.monotonic()
gauges = {}
samples: Dict[str, List[float]] = defaultdict(list)
while True:
started_prev = sm['deviceState'].started
sm.update()
# Update metrics
try:
while True:
try:
metric = sock.recv_string(zmq.NOBLOCK)
started_prev = sm['deviceState'].started
sm.update()
# Update metrics
while True:
try:
metric_type = metric.split('|')[1]
metric_name = metric.split(':')[0]
metric_value = float(metric.split('|')[0].split(':')[1])
metric = sock.recv_string(zmq.NOBLOCK)
try:
metric_type = metric.split('|')[1]
metric_name = metric.split(':')[0]
metric_value = float(metric.split('|')[0].split(':')[1])
if metric_type == METRIC_TYPE.GAUGE:
gauges[metric_name] = metric_value
elif metric_type == METRIC_TYPE.SAMPLE:
samples[metric_name].append(metric_value)
else:
cloudlog.event("unknown metric type", metric_type=metric_type)
except Exception:
cloudlog.event("malformed metric", metric=metric)
except zmq.error.Again:
break
if metric_type == METRIC_TYPE.GAUGE:
gauges[metric_name] = metric_value
elif metric_type == METRIC_TYPE.SAMPLE:
samples[metric_name].append(metric_value)
else:
cloudlog.event("unknown metric type", metric_type=metric_type)
except Exception:
cloudlog.event("malformed metric", metric=metric)
except zmq.error.Again:
break
# flush when started state changes or after FLUSH_TIME_S
if (time.monotonic() > last_flush_time + STATS_FLUSH_TIME_S) or (sm['deviceState'].started != started_prev):
result = ""
current_time = datetime.utcnow().replace(tzinfo=timezone.utc)
tags['started'] = sm['deviceState'].started
# flush when started state changes or after FLUSH_TIME_S
if (time.monotonic() > last_flush_time + STATS_FLUSH_TIME_S) or (sm['deviceState'].started != started_prev):
result = ""
current_time = datetime.utcnow().replace(tzinfo=timezone.utc)
tags['started'] = sm['deviceState'].started
for key, value in gauges.items():
result += get_influxdb_line(f"gauge.{key}", value, current_time, tags)
for key, value in gauges.items():
result += get_influxdb_line(f"gauge.{key}", value, current_time, tags)
for key, values in samples.items():
values.sort()
sample_count = len(values)
sample_sum = sum(values)
for key, values in samples.items():
values.sort()
sample_count = len(values)
sample_sum = sum(values)
stats = {
'count': sample_count,
'min': values[0],
'max': values[-1],
'mean': sample_sum / sample_count,
}
for percentile in [0.05, 0.5, 0.95]:
value = values[int(round(percentile * (sample_count - 1)))]
stats[f"p{int(percentile * 100)}"] = value
stats = {
'count': sample_count,
'min': values[0],
'max': values[-1],
'mean': sample_sum / sample_count,
}
for percentile in [0.05, 0.5, 0.95]:
value = values[int(round(percentile * (sample_count - 1)))]
stats[f"p{int(percentile * 100)}"] = value
result += get_influxdb_line(f"sample.{key}", stats, current_time, tags)
result += get_influxdb_line(f"sample.{key}", stats, current_time, tags)
# clear intermediate data
gauges.clear()
samples.clear()
last_flush_time = time.monotonic()
# clear intermediate data
gauges.clear()
samples.clear()
last_flush_time = time.monotonic()
# check that we aren't filling up the drive
if len(os.listdir(STATS_DIR)) < STATS_DIR_FILE_LIMIT:
if len(result) > 0:
stats_path = os.path.join(STATS_DIR, f"{current_time.timestamp():.0f}_{idx}")
with atomic_write_in_dir(stats_path) as f:
f.write(result)
idx += 1
else:
cloudlog.error("stats dir full")
# check that we aren't filling up the drive
if len(os.listdir(STATS_DIR)) < STATS_DIR_FILE_LIMIT:
if len(result) > 0:
stats_path = os.path.join(STATS_DIR, f"{current_time.timestamp():.0f}_{idx}")
with atomic_write_in_dir(stats_path) as f:
f.write(result)
idx += 1
else:
cloudlog.error("stats dir full")
finally:
sock.close()
ctx.term()
if __name__ == "__main__":
selfdrive/test/ciui.py
Executable
+58
View File
@@ -0,0 +1,58 @@
#!/usr/bin/env python3
import signal
import subprocess
signal.signal(signal.SIGINT, signal.SIG_DFL)
signal.signal(signal.SIGTERM, signal.SIG_DFL)
from PyQt5.QtCore import QTimer # pylint: disable=no-name-in-module, import-error
from PyQt5.QtWidgets import QApplication, QWidget, QVBoxLayout, QLabel # pylint: disable=no-name-in-module, import-error
from selfdrive.ui.qt.python_helpers import set_main_window
class Window(QWidget):
def __init__(self, parent=None):
super(Window, self).__init__(parent)
layout = QVBoxLayout()
self.setLayout(layout)
self.l = QLabel("jenkins runner")
layout.addWidget(self.l)
layout.addStretch(1)
layout.setContentsMargins(20, 20, 20, 20)
cmds = [
"cat /etc/hostname",
"echo AGNOS v$(cat /VERSION)",
"uptime -p",
]
self.labels = {}
for c in cmds:
self.labels[c] = QLabel(c)
layout.addWidget(self.labels[c])
self.setStyleSheet("""
* {
color: white;
font-size: 55px;
background-color: black;
font-family: "JetBrains Mono";
}
""")
self.timer = QTimer()
self.timer.timeout.connect(self.update)
self.timer.start(10 * 1000)
self.update()
def update(self):
for cmd, label in self.labels.items():
out = subprocess.run(cmd, stdout=subprocess.PIPE, stderr=subprocess.PIPE,
shell=True, check=False, encoding='utf8').stdout
label.setText(out.strip())
if __name__ == "__main__":
app = QApplication([])
w = Window()
set_main_window(w)
app.exec_()
selfdrive/test/process_replay/compare_logs.py
+1 -1
View File
@@ -70,7 +70,7 @@ def compare_logs(log1, log2, ignore_fields=None, ignore_msgs=None, tolerance=Non
raise Exception(f"logs are not same length: {len(log1)} VS {len(log2)}\n\t\t{cnt1}\n\t\t{cnt2}")
diff = []
for msg1, msg2 in zip(log1, log2):
for msg1, msg2 in zip(log1, log2, strict=True):
if msg1.which() != msg2.which():
raise Exception("msgs not aligned between logs")
selfdrive/test/process_replay/process_replay.py
+3 -3
View File
@@ -465,8 +465,8 @@ CONFIGS = [
subs=["radarState", "liveTracks"],
ignore=["logMonoTime", "valid", "radarState.cumLagMs"],
init_callback=get_car_params_callback,
should_recv_callback=MessageBasedRcvCallback("can"),
main_pub="can",
should_recv_callback=MessageBasedRcvCallback("modelV2"),
unlocked_pubs=["can"],
),
ProcessConfig(
proc_name="plannerd",
@@ -672,8 +672,8 @@ def _replay_multi_process(
containers = []
for cfg in cfgs:
container = ProcessContainer(cfg)
container.start(params_config, env_config, all_msgs, fingerprint, captured_output_store is not None)
containers.append(container)
container.start(params_config, env_config, all_msgs, fingerprint, captured_output_store is not None)
all_pubs = {pub for container in containers for pub in container.pubs}
all_subs = {sub for container in containers for sub in container.subs}
selfdrive/test/process_replay/ref_commit
+1 -1
View File
@@ -1 +1 @@
8fd42f02d1ecac695327bfc0afd4be1918ab680a
ac3f87bad519df408a295d2e007e0de58b11690c
selfdrive/test/process_replay/test_debayer.py
+2 -2
View File
@@ -137,7 +137,7 @@ if __name__ == "__main__":
failed = True
diff += 'amount of frames not equal\n'
for i, (frame, cmp_frame) in enumerate(zip(frames, cmp_frames)):
for i, (frame, cmp_frame) in enumerate(zip(frames, cmp_frames, strict=True)):
for j in range(3):
fr = frame[j]
cmp_f = cmp_frame[j]
@@ -159,7 +159,7 @@ if __name__ == "__main__":
diff += f'different at a large amount of pixels ({diff_len})\n'
else:
diff += 'different at (frame, yuv, pixel, ref, HEAD):\n'
for k in zip(*np.nonzero(frame_diff)):
for k in zip(*np.nonzero(frame_diff), strict=True):
diff += f'{i}, {yuv_i[j]}, {k}, {cmp_f[k]}, {fr[k]}\n'
if failed:

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